#if (R2D_PIXEL_DEPTH == 32)
#define R2D_PIXEL_DOWN_OUT(a) (0)
#define R2D_PIXEL_UP_OUT(a) (0)
#else
#define R2D_PIXEL_DOWN_OUT(a) (a>>R2D_PIXEL_DEPTH)
#define R2D_PIXEL_UP_OUT(a) (a<<R2D_PIXEL_DEPTH)
#endif

#if (R2D_ASM == R2D_ON)

// All below global are modified in the context of a blit_rect and
// the use of the self modifying blit_rect is protected by
// a semaphore

extern T_R2D_DRAWING_MODE r2d_g_old_mode;
extern R2D_BOOLEAN r2d_g_old_use_foreground;
extern INT32 r2d_g_old_srcdst;
extern UINT32 r2d_g_old_foreground_pixelvalue;
extern UINT32 r2d_g_old_background_pixelvalue;

extern void r2d_blit_la(UINT32 *a);
extern void r2d_blit_lb(UINT32 *a);

#endif

extern BOOLEAN IND_r2d_color_framebuffer_foreground_pixel(UINT32 lcd_value,T_R2D_GC_PTR src_gc);


#define r2d_get_texture_color() ((T_R2D_ANCHORED_TEXTURE*)(gc->background_texture))->pattern[((x-gc->s_org_x)&texture_mask)*texture_size+((ty-gc->s_org_y)&texture_mask)]






void r2d_write_lcd_line(T_R2D_GC* gc,INT16 x,INT16 y,INT16 nb,R2D_BOOLEAN background)
{
  UINT32 *p;
  UINT16 tmp,ty;
//  UINT32 temp;
  UINT32 pixel_cache,pixel_value;
  INT16 current_y,count;
  UINT32 new_value,current_value;
  T_R2D_DRAWING_OP dop;
  INT16 texture_mask,texture_size;

  ty=y; // True y

  if (gc->background_texture!=NULL)
  {
	  texture_mask=~((-1)<<gc->background_texture->size);
	  texture_size=1<<gc->background_texture->size;
  }

  dop=gc->drawing_op;
  
  if (nb==0) 
   goto r2d_fail_line;

  p=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words;

  switch(((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->kind)
  {
    case 0:// LCD format with hard coded dimensions

		{

       // Get position of the memory word containing the pixel
#if (R2D_REFRESH == R2D_VERTICAL)
       p+=((x*R2D_MWHEIGHT+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#else
	   p+=((x*R2D_MWWIDTH+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#endif

         if ((y<0) || (x<0) || ((p<((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words) 
         || (p>((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_frame_buffer_end)))
         {
           #if (R2D_DEBUG == R2D_ON)
              //printf("Error : frame_buffer overflow\n");
			  IND_rvf_send_trace("R2D : Framebuffer overflow",26, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );

           #endif
           goto r2d_fail_line;
         }



       // Get the pixel position into the memory word
	   new_value=0;
       y=y & R2D_WORD_POSITION_MASK;
	   y=y << R2D_PIXEL_POS_TO_BIT_POS;
       pixel_cache=*p;
       current_y=0;
       current_value=pixel_cache;
       if (y!=0)
       {
          do
          {
            new_value =R2D_PIXEL_DOWN_OUT(new_value);
            new_value|=((current_value & R2D_PIXEL_MASK)
              << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) ;
            current_value=R2D_PIXEL_DOWN_OUT(current_value);
            current_y+=(1<<R2D_PIXEL_POS_TO_BIT_POS);
          } while (current_y != y);
       }
       count=0;
	   #if (R2D_DITHERING == R2D_OFF)
             if (background)
			 	pixel_value=((T_R2D_GC*)gc)->background_pixel_value;
			 else
				 pixel_value=((T_R2D_GC*)gc)->foreground_pixel_value;
       #endif
       do
       {
		 #if (R2D_DITHERING == R2D_ON)
           if (background)
		   {
			 if (gc->background_texture!=NULL)
			   pixel_value=r2d_get_texture_color();
             else
			   pixel_value=r2d_get_dithering_matrix_entry(((T_R2D_GC*)gc)->p_background_dithered_cache,x,ty);
            
		   }
		   else
             pixel_value=r2d_get_dithering_matrix_entry(((T_R2D_GC*)gc)->p_foreground_dithered_cache,x,ty);
         #endif

         #if (R2D_DITHERING == R2D_OFF)
		   if ((background) && (gc->background_texture!=NULL))
			pixel_value=r2d_get_texture_color();
         #endif
		 
		 ty++;

         new_value =R2D_PIXEL_DOWN_OUT(new_value);
		 
         new_value|=((dop(current_value & R2D_PIXEL_MASK,pixel_value) & R2D_PIXEL_MASK)
           << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) ;
         current_value=R2D_PIXEL_DOWN_OUT(current_value);
         current_y+=(1<<R2D_PIXEL_POS_TO_BIT_POS);
         if (current_y==(1<<R2D_MEMORY_WORD))
         {
            current_y=0;
            *p++=new_value;
            current_value=*p;
         }
         count++;
      } while (count<nb);

       while(current_y != (1<<R2D_MEMORY_WORD))
       {
         new_value =R2D_PIXEL_DOWN_OUT(new_value) ;
         new_value|=((current_value & R2D_PIXEL_MASK)
           << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) ;
         current_value=R2D_PIXEL_DOWN_OUT(current_value);
         current_y+=(1<<R2D_PIXEL_POS_TO_BIT_POS);
       }
       *p++=new_value;

  }
    break;
    case R2D_LCD_KIND: // LCD format with any size
    {
       INT16 height,width;
       height=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->height;
       width=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->width;
      
       // Get position of the memory word containing the pixel
	#if (R2D_REFRESH == R2D_VERTICAL)
       p+=((x*R2D_ALIGNED_MWLENGTH(height)+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#else
	   p+=((x*R2D_ALIGNED_MWLENGTH(width)+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#endif

         if ((y<0) || (x<0) || ((p<((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words) 
         || (p>((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_frame_buffer_end)))
         {
           #if (R2D_DEBUG == R2D_ON)
              IND_rvf_send_trace("R2D : Framebuffer overflow",26, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );
           #endif
           goto r2d_fail_line;
         }



       // Get the pixel position into the memory word
	   new_value=0;
       y=y & R2D_WORD_POSITION_MASK;
	   y=y << R2D_PIXEL_POS_TO_BIT_POS;
       pixel_cache=*p;
       current_y=0;
       current_value=pixel_cache;
       if (y!=0)
       {
          do
          {
            new_value =R2D_PIXEL_DOWN_OUT(new_value) ;
            new_value|=((current_value & R2D_PIXEL_MASK)
              << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) ;
            current_value=R2D_PIXEL_DOWN_OUT(current_value);
            current_y+=(1<<R2D_PIXEL_POS_TO_BIT_POS);
          } while (current_y != y);
       }
       count=0;
	   #if (R2D_DITHERING == R2D_OFF)
             if (background)
				 pixel_value=((T_R2D_GC*)gc)->background_pixel_value;
			 else
				 pixel_value=((T_R2D_GC*)gc)->foreground_pixel_value;
       #endif
       do
       {
		 #if (R2D_DITHERING == R2D_ON)
           if (background)
			  if (gc->background_texture!=NULL)
                pixel_value=r2d_get_texture_color();
			  else
                pixel_value=r2d_get_dithering_matrix_entry(((T_R2D_GC*)gc)->p_background_dithered_cache,x,ty);   
           else
             pixel_value=r2d_get_dithering_matrix_entry(((T_R2D_GC*)gc)->p_foreground_dithered_cache,x,ty);      
         #endif

         #if (R2D_DITHERING == R2D_OFF)
            if ((background) && (gc->background_texture!=NULL))
			   pixel_value=r2d_get_texture_color();
            #endif

		 ty++;

         new_value =R2D_PIXEL_DOWN_OUT(new_value);
         new_value|=((dop(current_value & R2D_PIXEL_MASK,pixel_value) & R2D_PIXEL_MASK)
           << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) ;
         current_value=R2D_PIXEL_DOWN_OUT(current_value);
         current_y+=(1<<R2D_PIXEL_POS_TO_BIT_POS);
         if (current_y==(1<<R2D_MEMORY_WORD))
         {
            current_y=0;
            *p++=new_value;
            current_value=*p;
         }
         count++;
      } while (count<nb);

       while(current_y != (1<<R2D_MEMORY_WORD))
       {
         new_value =R2D_PIXEL_DOWN_OUT(new_value);
         new_value|=((current_value & R2D_PIXEL_MASK)
           << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) ;
         current_value=R2D_PIXEL_DOWN_OUT(current_value);
         current_y+=(1<<R2D_PIXEL_POS_TO_BIT_POS);
       }
       *p++=new_value;

   
    }
       break;

    case R2D_FULL_KIND: // LCD format with any size
    {
       INT16 height,width;
       height=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->height;
       width=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->width;
       

       // Get position of the memory word containing the pixel
#if (R2D_REFRESH == R2D_VERTICAL)
       p+=((x*height+y));
#else
	   p+=((x*width+y));
#endif

         if ((y<0) || (x<0) || ((p<((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words) 
         || (p>((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_frame_buffer_end)))
         {
           #if (R2D_DEBUG == R2D_ON)
              IND_rvf_send_trace("R2D : Framebuffer overflow",26, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );
           #endif
           goto r2d_fail_line;
         }



       // Get the pixel position into the memory word
       y=0;
	   new_value=0;
       pixel_cache=*p;
       current_y=0;
       current_value=pixel_cache;
       count=0;
       if (background)
       {
           pixel_value=((T_R2D_GC*)gc)->background_pixel_value;
       }
       else
       {
           pixel_value=((T_R2D_GC*)gc)->foreground_pixel_value;
       }

       do
       {
		 
            if ((background) && (gc->background_texture!=NULL))
			   pixel_value=r2d_get_texture_color();

		 ty++;
         new_value=dop(current_value,pixel_value);
         current_y=0;
         *p++=new_value;
         current_value=*p;
         count++;
      } while (count<nb);


    
    }
       break;
  }
  r2d_fail_line:tmp=0; // Just because one needs code after a label
}

void r2d_arc_write_lcd_line(T_R2D_GC* gc,INT16 x,INT16 y,
							INT16 org_x,INT16 org_y,INT16 nb,
							T_R2D_ARC_REGION *rgn,R2D_BOOLEAN background)
{
  UINT32 *p;
  UINT16 tmp,ty;
//  UINT32 temp;
  UINT32 pixel_cache,pixel_value;
  INT32 current_y,count;
  UINT32 new_value,current_value;
  T_R2D_DRAWING_OP dop;
  INT16 texture_mask,texture_size;
  INT32 sides,sidee;
  BOOLEAN start_filling=FALSE;

  sides=r2d_get_point_side((INT16)(x-org_x),(INT16)(y-org_y),rgn->sa,rgn->sb);
  sidee=r2d_get_point_side((INT16)(x-org_x),(INT16)(y-org_y),rgn->ea,rgn->eb);

  ty=y; // True y

  if (gc->background_texture!=NULL)
  {
	  texture_mask=~((-1)<<gc->background_texture->size);
	  texture_size=1<<gc->background_texture->size;
  }

  dop=gc->drawing_op;
  
  if (nb==0) 
   goto r2d_fail_line;

  p=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words;

  switch(((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->kind)
  {
    case 0:// LCD format with hard coded dimensions

		{

       // Get position of the memory word containing the pixel
#if (R2D_REFRESH == R2D_VERTICAL)
       p+=((x*R2D_MWHEIGHT+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#else
	   p+=((x*R2D_MWWIDTH+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#endif

         if ((y<0) || (x<0) || ((p<((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words) 
         || (p>((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_frame_buffer_end)))
         {
           #if (R2D_DEBUG == R2D_ON)
              //printf("Error : frame_buffer overflow\n");
			  IND_rvf_send_trace("R2D : Framebuffer overflow",26, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );

           #endif
           goto r2d_fail_line;
         }



       // Get the pixel position into the memory word
	   new_value=0;
       y=y & R2D_WORD_POSITION_MASK;
	   y=y << R2D_PIXEL_POS_TO_BIT_POS;
       pixel_cache=*p;
       current_y=0;
       current_value=pixel_cache;
       if (y!=0)
       {
          do
          {
            new_value =R2D_PIXEL_DOWN_OUT(new_value);
            new_value|=((current_value & R2D_PIXEL_MASK)
              << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) ;
            current_value=R2D_PIXEL_DOWN_OUT(current_value);
            current_y+=(1<<R2D_PIXEL_POS_TO_BIT_POS);
          } while (current_y != y);
       }
       count=0;
	   #if (R2D_DITHERING == R2D_OFF)
             if (background)
			 	pixel_value=((T_R2D_GC*)gc)->background_pixel_value;
			 else
				 pixel_value=((T_R2D_GC*)gc)->foreground_pixel_value;
       #endif
       do
       {
		 #if (R2D_DITHERING == R2D_ON)
           if (background)
		   {
			 if (gc->background_texture!=NULL)
			   pixel_value=r2d_get_texture_color();
             else
			   pixel_value=r2d_get_dithering_matrix_entry(((T_R2D_GC*)gc)->p_background_dithered_cache,x,ty);
            
		   }
		   else
             pixel_value=r2d_get_dithering_matrix_entry(((T_R2D_GC*)gc)->p_foreground_dithered_cache,x,ty);
         #endif

         #if (R2D_DITHERING == R2D_OFF)
		   if ((background) && (gc->background_texture!=NULL))
			pixel_value=r2d_get_texture_color();
         #endif
		 
		 

         new_value =R2D_PIXEL_DOWN_OUT(new_value);
		 
		 if (
			 ((rgn->one_sector==0) && (sides>=0) && (sidee>=0))
			 || 
			 ((rgn->one_sector==1) && (!(!(sides>=0) && !(sidee>=0))))
			 || (rgn->one_sector==2)
			 )
		 {
			 if (rgn->one_sector==0)
			start_filling=TRUE;
            new_value|=((dop(current_value & R2D_PIXEL_MASK,pixel_value) & R2D_PIXEL_MASK)
              << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) ;
		 }
		 else
		 {
		   if (start_filling)
			   goto r2d_fail_line;
           new_value|=(current_value & R2D_PIXEL_MASK)
           << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH) ;
		 }

		 ty++;
		 sides+=-(rgn->sa>>1);
		 sidee+=-(rgn->ea>>1);

         current_value=R2D_PIXEL_DOWN_OUT(current_value);
         current_y+=(1<<R2D_PIXEL_POS_TO_BIT_POS);
         if (current_y==(1<<R2D_MEMORY_WORD))
         {
            current_y=0;
            *p++=new_value;
            current_value=*p;
         }
         count++;
      } while (count<nb);

       while(current_y != (1<<R2D_MEMORY_WORD))
       {
         new_value =R2D_PIXEL_DOWN_OUT(new_value) ;
         new_value|=((current_value & R2D_PIXEL_MASK)
           << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) ;
         current_value=R2D_PIXEL_DOWN_OUT(current_value);
         current_y+=(1<<R2D_PIXEL_POS_TO_BIT_POS);
       }
       *p++=new_value;

  }
    break;
    case R2D_LCD_KIND: // LCD format with any size
    {
       INT16 height,width;
       height=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->height;
       width=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->width;
      
       // Get position of the memory word containing the pixel
	#if (R2D_REFRESH == R2D_VERTICAL)
       p+=((x*R2D_ALIGNED_MWLENGTH(height)+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#else
	   p+=((x*R2D_ALIGNED_MWLENGTH(width)+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#endif

         if ((y<0) || (x<0) || ((p<((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words) 
         || (p>((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_frame_buffer_end)))
         {
           #if (R2D_DEBUG == R2D_ON)
              IND_rvf_send_trace("R2D : Framebuffer overflow",26, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );
           #endif
           goto r2d_fail_line;
         }



       // Get the pixel position into the memory word
	   new_value=0;
       y=y & R2D_WORD_POSITION_MASK;
	   y=y << R2D_PIXEL_POS_TO_BIT_POS;
       pixel_cache=*p;
       current_y=0;
       current_value=pixel_cache;
       if (y!=0)
       {
          do
          {
            new_value =R2D_PIXEL_DOWN_OUT(new_value) ;
            new_value|=((current_value & R2D_PIXEL_MASK)
              << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) ;
            current_value=R2D_PIXEL_DOWN_OUT(current_value);
            current_y+=(1<<R2D_PIXEL_POS_TO_BIT_POS);
          } while (current_y != y);
       }
       count=0;
	   #if (R2D_DITHERING == R2D_OFF)
             if (background)
				 pixel_value=((T_R2D_GC*)gc)->background_pixel_value;
			 else
				 pixel_value=((T_R2D_GC*)gc)->foreground_pixel_value;
       #endif
       do
       {
		 #if (R2D_DITHERING == R2D_ON)
           if (background)
			  if (gc->background_texture!=NULL)
                pixel_value=r2d_get_texture_color();
			  else
                pixel_value=r2d_get_dithering_matrix_entry(((T_R2D_GC*)gc)->p_background_dithered_cache,x,ty);   
           else
             pixel_value=r2d_get_dithering_matrix_entry(((T_R2D_GC*)gc)->p_foreground_dithered_cache,x,ty);      
         #endif

         #if (R2D_DITHERING == R2D_OFF)
            if ((background) && (gc->background_texture!=NULL))
			   pixel_value=r2d_get_texture_color();
            #endif

		 

         new_value =R2D_PIXEL_DOWN_OUT(new_value);

		 if (
			 ((rgn->one_sector==0) && (sides>=0) && (sidee>=0))
			 || 
			 ((rgn->one_sector==1) && (!(!(sides>=0) && !(sidee>=0))))
			 || (rgn->one_sector==2)
			 )
		 {
			 if (rgn->one_sector==0)
			 start_filling=TRUE;
           new_value|=((dop(current_value & R2D_PIXEL_MASK,pixel_value) & R2D_PIXEL_MASK)
             << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) ;
         }
		  else
		  {
			  if (start_filling)
			   goto r2d_fail_line;
          new_value|=((current_value & R2D_PIXEL_MASK)
             << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) ;
		  }
		 ty++;
		 sides+=-(rgn->sa>>1);
		 sidee+=-(rgn->ea>>1);

		 current_value=R2D_PIXEL_DOWN_OUT(current_value);
         current_y+=(1<<R2D_PIXEL_POS_TO_BIT_POS);
         if (current_y==(1<<R2D_MEMORY_WORD))
         {
            current_y=0;
            *p++=new_value;
            current_value=*p;
         }
         count++;
      } while (count<nb);

       while(current_y != (1<<R2D_MEMORY_WORD))
       {
         new_value =R2D_PIXEL_DOWN_OUT(new_value);
         new_value|=((current_value & R2D_PIXEL_MASK)
           << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) ;
         current_value=R2D_PIXEL_DOWN_OUT(current_value);
         current_y+=(1<<R2D_PIXEL_POS_TO_BIT_POS);
       }
       *p++=new_value;

   
    }
       break;

    case R2D_FULL_KIND: // LCD format with any size
    {
       INT16 height,width;
       height=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->height;
       width=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->width;
       

       // Get position of the memory word containing the pixel
#if (R2D_REFRESH == R2D_VERTICAL)
       p+=((x*height+y));
#else
	   p+=((x*width+y));
#endif

         if ((y<0) || (x<0) || ((p<((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words) 
         || (p>((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_frame_buffer_end)))
         {
           #if (R2D_DEBUG == R2D_ON)
              IND_rvf_send_trace("R2D : Framebuffer overflow",26, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );
           #endif
           goto r2d_fail_line;
         }



       // Get the pixel position into the memory word
       y=0;
	   new_value=0;
       pixel_cache=*p;
       current_y=0;
       current_value=pixel_cache;
       count=0;
       if (background)
       {
           pixel_value=((T_R2D_GC*)gc)->background_pixel_value;
       }
       else
       {
           pixel_value=((T_R2D_GC*)gc)->foreground_pixel_value;
       }

       do
       {
		 
            if ((background) && (gc->background_texture!=NULL))
			   pixel_value=r2d_get_texture_color();

		 
         if (
			 ((rgn->one_sector==0) && (sides>=0) && (sidee>=0))
			 || 
			 ((rgn->one_sector==1) && (!(!(sides>=0) && !(sidee>=0))))
			 || (rgn->one_sector==2)
			 )
		 {
			 if (rgn->one_sector==0)
			 start_filling=TRUE;
            new_value=dop(current_value,pixel_value);
		 }
         else
		 {
			 if (start_filling)
			   goto r2d_fail_line;
            new_value=current_value;
		 }
		 ty++;
		 sides+=-(rgn->sa>>1);
		 sidee+=-(rgn->ea>>1);

         current_y=0;
         *p++=new_value;
         current_value=*p;
         count++;
      } while (count<nb);


    
    }
       break;
  }
  r2d_fail_line:tmp=0; // Just because one needs code after a label
}

// Returns the equivalent color pixel value
// for the color kind of framebuffer
// It must be converted to RGB components
// Don't confuse with r2d_get_pixel_value which is
// just reading the cached pixel vlue from
// the graphic context.
// That routine is extracting the pixel value at position (x,y)
// from framebuffer
UINT32 r2d_get_color_pixel_value(T_R2D_GC* gc,INT16 x,INT16 y)
{
  UINT32 *p;
//  UINT16 tmp;
  UINT32 pixel_cache;//,new_value;
  UINT32 result;

  p=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words;

  switch(((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->kind)
  {
    case 0: // LCD format with hard coded dimensions
		{
        // Get position of the memory word containing the pixel
#if (R2D_REFRESH == R2D_VERTICAL)
        p+=((x*R2D_MWHEIGHT+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#else
		p+=((x*R2D_MWWIDTH+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#endif

          if ((y<0) || (x<0) || ((p<((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words) 
          || (p>((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_frame_buffer_end)))
          {
            #if (R2D_DEBUG == R2D_ON)
               IND_rvf_send_trace("R2D : Framebuffer overflow",26, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );
            #endif
            goto r2d_fail_get_pixel;
          }


        // Get the pixel position into the memory word
        y=y & R2D_WORD_POSITION_MASK;
		y=y << R2D_PIXEL_POS_TO_BIT_POS;
        pixel_cache=*p;

        result=(pixel_cache >> y) & R2D_PIXEL_MASK;
        return((~IND_r2d_lcd_to_color(result)) & 0x00FFFFFF);
		}
    break;
    case R2D_LCD_KIND: // LCD format with any size
    {
      INT16 height,width;
      height=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->height;
      width=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->width;
     
#if (R2D_REFRESH == R2D_VERTICAL)
        // Get position of the memory word containing the pixel
        p+=((x*R2D_ALIGNED_MWLENGTH(height)+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#else
		p+=((x*R2D_ALIGNED_MWLENGTH(width)+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#endif


          if ((y<0) || (x<0) || ((p<((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words) 
          || (p>((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_frame_buffer_end)))
          {
            #if (R2D_DEBUG == R2D_ON)
               IND_rvf_send_trace("R2D : Framebuffer overflow",26, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );
            #endif
            goto r2d_fail_get_pixel;
          }


        // Get the pixel position into the memory word
        y=y & R2D_WORD_POSITION_MASK;
		y=y << R2D_PIXEL_POS_TO_BIT_POS;
        pixel_cache=*p;

        result=(pixel_cache >> y) & R2D_PIXEL_MASK;
        
        return(~IND_r2d_lcd_to_color(result) & 0x00FFFFFF);
    }
    break;
    case R2D_FULL_KIND:
    {
      INT16 height,width;
      height=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->height;
      width=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->width;
     
       //printf("%08X\n",p);
        // Get position of the memory word containing the pixel
#if (R2D_REFRESH == R2D_VERTICAL)
        p+=(x*height+y);
#else
		p+=(x*width+y);
#endif

       //printf(" --> %08X for x=%d and y=%d\n",p,x,y);


          if ((y<0) || (x<0) || ((p<((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words) 
          || (p>((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_frame_buffer_end)))
          {
            #if (R2D_DEBUG == R2D_ON)
               IND_rvf_send_trace("R2D : Framebuffer overflow",26, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );
            #endif
            goto r2d_fail_get_pixel;
          }


        // Get the pixel position into the memory word
        y=0;
        pixel_cache=*p;

        result=(~pixel_cache) & 0x00FFFFFF;
	}
    break;
  }
r2d_fail_get_pixel:return(0);
}

// Low level pixel drawing
// (Note that for filling a more efficient version is used
// taking into account the fact that the position
// in the framebuffer has not to be recomputed from the coordinates
// for each pixel)
void r2d_write_lcd_pixel(T_R2D_GC* gc,INT16 x,INT16 y,UINT32 pixel_value)
{
  UINT32 *p;
  UINT16 tmp;
  UINT32 pixel_cache,new_value;
  T_R2D_DRAWING_OP dop;

  dop=gc->drawing_op;

  p=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words;

  switch(((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->kind)
  {
    case 0: // LCD format with hard coded dimensions
		{
        // Get position of the memory word containing the pixel
#if (R2D_REFRESH == R2D_VERTICAL)
        p+=((x*R2D_MWHEIGHT+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#else
		p+=((x*R2D_MWWIDTH+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#endif

          if ((y<0) || (x<0) || ((p<((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words) 
          || (p>((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_frame_buffer_end)))
          {
            #if (R2D_DEBUG == R2D_ON)
               IND_rvf_send_trace("R2D : Framebuffer overflow",26, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );
            #endif
            goto r2d_fail_pixel;
          }


        // Get the pixel position into the memory word
        y=y & R2D_WORD_POSITION_MASK;
		y=y << R2D_PIXEL_POS_TO_BIT_POS;
        pixel_cache=*p;

        new_value=dop((pixel_cache >> y) & R2D_PIXEL_MASK,pixel_value) & R2D_PIXEL_MASK;
        pixel_cache &= ~(R2D_PIXEL_MASK << y) ;

        // Write new value
        pixel_cache |= (new_value << y);
        *p=pixel_cache;
		}
    break;
    case R2D_LCD_KIND: // LCD format with any size
    {
      INT16 height,width;
      height=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->height;
      width=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->width;
     
#if (R2D_REFRESH == R2D_VERTICAL)
        // Get position of the memory word containing the pixel
        p+=((x*R2D_ALIGNED_MWLENGTH(height)+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#else
		p+=((x*R2D_ALIGNED_MWLENGTH(width)+(y>>R2D_PIXELS_PER_MEMORY_WORD)));
#endif


          if ((y<0) || (x<0) || ((p<((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words) 
          || (p>((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_frame_buffer_end)))
          {
            #if (R2D_DEBUG == R2D_ON)
               IND_rvf_send_trace("R2D : Framebuffer overflow",26, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );
            #endif
            goto r2d_fail_pixel;
          }


        // Get the pixel position into the memory word
        y=y & R2D_WORD_POSITION_MASK;
		y=y << R2D_PIXEL_POS_TO_BIT_POS;
        pixel_cache=*p;

        new_value=dop((pixel_cache >> y) & R2D_PIXEL_MASK,pixel_value) & R2D_PIXEL_MASK;
        pixel_cache &= ~(R2D_PIXEL_MASK << y) ;

        // Write new value
        pixel_cache |= (new_value << y);
        *p=pixel_cache;
     
    }
    break;
    case R2D_FULL_KIND:
    {
      INT16 height,width;
      height=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->height;
      width=((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->width;
     
       //printf("%08X\n",p);
        // Get position of the memory word containing the pixel
#if (R2D_REFRESH == R2D_VERTICAL)
        p+=(x*height+y);
#else
		p+=(x*width+y);
#endif

       //printf(" --> %08X for x=%d and y=%d\n",p,x,y);


          if ((y<0) || (x<0) || ((p<((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_memory_words) 
          || (p>((T_R2D_FRAMEBUFFER*)(gc->p_frame_buffer))->p_frame_buffer_end)))
          {
            #if (R2D_DEBUG == R2D_ON)
               IND_rvf_send_trace("R2D : Framebuffer overflow",26, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );
            #endif
            goto r2d_fail_pixel;
          }


        // Get the pixel position into the memory word
        y=0;
        pixel_cache=*p;

        new_value=dop(pixel_cache,pixel_value);
        pixel_cache = new_value;
        *p=pixel_cache;
	}
    break;
  }
  r2d_fail_pixel:tmp=0; // Just because one needs code after a label
}




#if (R2D_ASM == R2D_OFF)



// For blitting, two scanning direction are required because of possible
// overlaps between src and dst

// Shift new pixel from srcstream to dststream using variable srccache
// as a pixel cache.dstcounter allows to keep track of number
// of pixels written
// (scanning direction is down)

#define r2d_shift_pixel_down(dststream,srccache,srccounter,srcstream) {dststream =R2D_PIXEL_DOWN_OUT(dststream); \
               dststream|=((srccache & R2D_PIXEL_MASK) \
               << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) ; \
               srccache=R2D_PIXEL_DOWN_OUT(srccache); \
               srccounter--; \
               if (srccounter==0) \
               { \
                 srccounter=(1<<R2D_PIXELS_PER_MEMORY_WORD); \
                 srccache=*++srcstream;  \
               } }


// Scanning direction is up
#define r2d_shift_pixel_up(dststream,srccache,srccounter,srcstream) {dststream =R2D_PIXEL_UP_OUT(dststream); \
               dststream |=(srccache >> ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) & R2D_PIXEL_MASK; \
               srccache=R2D_PIXEL_UP_OUT(srccache); \
               srccounter--; \
               if (srccounter==0) \
               { \
                 srccounter=(1<<R2D_PIXELS_PER_MEMORY_WORD); \
                 srccache=*--srcstream;  \
               } }

#endif

// Check overlap of src and dst rectangle and return the
// horizontal and vertical scanning direction required to do the
// blit rect
 
// A positive value means standard direction (increasing x and increasing y)
// When that routine is called, both rectangles are expressed in framebuffer
// coordinates and come from the same framebuffer.
// The routine is never called if dstGc and srcGc are not using the same framebuffer.
//
// INT32 used because of stack bug ?
void r2d_check_rectangle_overlap(INT16 src_x,INT16 src_y,INT16 dst_x,INT16 dst_y,
INT32 width,INT32 height,
INT32 *h_direction,INT32 *v_direction)
{
   INT16 h,v;
   INT16 rx,ry;
   rx=dst_x-src_x;
   ry=dst_y-src_y;

   h=1;
   v=1;

   if ((rx>-width) && (rx < width) && (ry>-height) && (ry<height))
   {
      if (rx>0) 
         h=-1;
      else
         h=1;
      if (ry>0) 
         v=-1;
      else
         v=1;
   }

   *h_direction=h;
   *v_direction=v;
}


#if (R2D_ASM == R2D_OFF)


static UINT32 r2d_get_pixel_value(T_R2D_GC_PTR gc,R2D_BOOLEAN foreground,INT16 x,INT16 y)
{
	UINT32 pixel_value;
	if (foreground)
	{
	 #if (R2D_DITHERING == R2D_OFF)
             pixel_value=((T_R2D_GC*)gc)->foreground_pixel_value;
     #else
             pixel_value=r2d_get_dithering_matrix_entry(((T_R2D_GC*)gc)->p_foreground_dithered_cache,x,y);
     #endif
	}
	else
	{
	 #if (R2D_DITHERING == R2D_OFF)
             pixel_value=((T_R2D_GC*)gc)->background_pixel_value;
     #else
             pixel_value=r2d_get_dithering_matrix_entry(((T_R2D_GC*)gc)->p_background_dithered_cache,x,y);
     #endif
	}
	return(pixel_value);
}

// When foreground color is used, the source pixel
// is converted to the background or foreground color
// then dithered
static UINT32 r2d_convert_from_lcd_to_lcd(R2D_BOOLEAN use_foreground,UINT32 src,T_R2D_GC_PTR src_gc,T_R2D_GC_PTR dst_gc,INT16 x,INT16 y)
{
 if (use_foreground)
 {
	 
     UINT32 pixel_value;
	 if (IND_r2d_lcd_foreground_pixel(src,src_gc))
	   pixel_value=r2d_get_pixel_value(dst_gc,TRUE,x,y);
	 else
       pixel_value=r2d_get_pixel_value(dst_gc,FALSE,x,y);

	 return(pixel_value);
 }
 else return(src);
}

static UINT32 r2d_convert_from_color_to_color(R2D_BOOLEAN use_foreground,UINT32 src,T_R2D_GC_PTR src_gc,T_R2D_GC_PTR dst_gc,INT16 x,INT16 y)
{
 if (use_foreground)
 {
	 
     UINT32 pixel_value;
	 if (IND_r2d_color_framebuffer_foreground_pixel(src,src_gc))
	   pixel_value=r2d_get_pixel_value(dst_gc,TRUE,x,y);
	 else
       pixel_value=r2d_get_pixel_value(dst_gc,FALSE,x,y);

	 return(pixel_value);
 }
 else return(src);
}


static UINT32 r2d_convert_from_color_to_lcd(R2D_BOOLEAN use_foreground,UINT32 src,T_R2D_GC_PTR src_gc,T_R2D_GC_PTR dst_gc,INT16 x,INT16 y)
{
 if (use_foreground)
 {
	 UINT32 pixel_value;
	 if (IND_r2d_color_framebuffer_foreground_pixel(src,src_gc))
	   pixel_value=r2d_get_pixel_value(dst_gc,TRUE,x,y);
	 else
       pixel_value=r2d_get_pixel_value(dst_gc,FALSE,x,y);

	return(pixel_value);
 }
 else return(IND_r2d_color_to_lcd(src,x,y));
}

static UINT32 r2d_convert_from_lcd_to_color(R2D_BOOLEAN use_foreground,UINT32 src,T_R2D_GC_PTR src_gc,T_R2D_GC_PTR dst_gc,INT16 x,INT16 y)
{
 if (use_foreground)
 {
	 UINT32 pixel_value;
	 if (IND_r2d_lcd_foreground_pixel(src,src_gc))
	   pixel_value=r2d_get_pixel_value(dst_gc,TRUE,x,y);
	 else
       pixel_value=r2d_get_pixel_value(dst_gc,FALSE,x,y);

	 return(pixel_value);
 }
 else 
 {
	 return(IND_r2d_lcd_to_color(src));
 }
}

// FOR DEBUG
//#define dop(a,b) b

#define r2d_shift_pixel_down_and_write(dst,src) {\
               dst##value =R2D_PIXEL_DOWN_OUT(dst##value); \
               dst##value|=dop((dst##_current & R2D_PIXEL_MASK), \
               r2d_convert_from_lcd_to_lcd(use_foreground_color,\
			   src##_current & R2D_PIXEL_MASK,src_gc,dst_gc,x,y)) \
               << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH) ; \
               src##_current=R2D_PIXEL_DOWN_OUT(src##_current); \
               dst##_current=R2D_PIXEL_DOWN_OUT(dst##_current); \
               dst##counter--; \
               src##counter--; \
               if (src##counter==0) \
               { \
                 src##counter=(1<<R2D_PIXELS_PER_MEMORY_WORD); \
                 src##_current=*++p_##src##_current;  \
               } \
               if (dst##counter==0) \
               { \
                 dst##counter=(1<<R2D_PIXELS_PER_MEMORY_WORD); \
               } \
               }

#define r2d_shift_pixel_up_and_write(dst,src) {\
               dst##value =R2D_PIXEL_UP_OUT(dst##value); \
               dst##value |=dop( \
               (dst##_current >> ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) & R2D_PIXEL_MASK, \
              r2d_convert_from_lcd_to_lcd(use_foreground_color,\
			   (src##_current >> ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) & R2D_PIXEL_MASK, \
			   src_gc,dst_gc,x,y)); \
               src##_current=R2D_PIXEL_UP_OUT(src##_current); \
               dst##_current=R2D_PIXEL_UP_OUT(dst##_current); \
               dst##counter--; \
               src##counter--; \
               if (src##counter==0) \
               { \
                 src##counter=(1<<R2D_PIXELS_PER_MEMORY_WORD); \
                 src##_current=*--p_##src##_current;  \
               } \
               if (dst##counter==0) \
               { \
                 dst##counter=(1<<R2D_PIXELS_PER_MEMORY_WORD); \
               } \
               }

extern UINT32 r2d_lcd_or_operator(UINT32 old,UINT32 value);
// Requires shapes in graphic context coordinates
void           r2d_blit_lcd_to_lcd(T_R2D_GC_PTR src_gc,T_R2D_GC_PTR dst_gc,
                             T_R2D_SHAPE_PTR src_rectangle, 
							 T_R2D_SHAPE_PTR dst_rectangle,
							 R2D_BOOLEAN use_foreground_color)
{
    UINT32 *p_src,*p_dst,*p_src_current,*p_dst_current,*p_src_start,*p_dst_start;
    INT16 rect_width,rect_height,src_x,src_y,dst_x,dst_y;
    INT16 src_offset,dst_offset,src_height,dst_height;
    INT16 xnb,ynb,src_nb_rows,dst_nb_rows;
    INT16 src_dy,dst_dy,end_dst_dy,end_src_dy; // distance from word boundary
//    INT16 shift; // relative position (modulo a memory word) between both rectangles
    // in bits
    INT16 current_src_dy,current_dst_dy;
    INT16 temp,dstcounter,srccounter;


    register UINT32 dstvalue,dst_current;
    register UINT32 src_current;//srcvalue;
    INT32  h_direction,v_direction;
	INT16 x,y;
    T_R2D_DRAWING_MODE mode;
	T_R2D_DRAWING_OP dop;
	BOOLEAN compensate=TRUE;

	
    dop=((T_R2D_GC*)dst_gc)->drawing_op;

    

    {

    p_src=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))->p_memory_words;
    p_dst=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->p_memory_words;

    rect_width=r2d_get_xmax(src_rectangle)-r2d_get_xmin(src_rectangle);
    rect_height=r2d_get_ymax(src_rectangle)-r2d_get_ymin(src_rectangle);
	if ((rect_width<=0) || (rect_height<=0))
		goto end_blit;

    mode=IND_r2d_get_drawing_mode(dst_gc);

    
    src_x=r2d_get_xmin(src_rectangle);
    src_y=r2d_get_ymin(src_rectangle);

    dst_x=r2d_get_xmin(dst_rectangle);
    dst_y=r2d_get_ymin(dst_rectangle);

    src_dy=(src_y & R2D_WORD_POSITION_MASK);
	dst_dy=(dst_y & R2D_WORD_POSITION_MASK);
	
    // Clipping convention such end_dst_dy is the first y position which must not
    // be copied at the end of a column
    //printf("dest y max=%d\n",r2d_get_ymax(dst_rectangle));
    end_dst_dy=((r2d_get_ymax(dst_rectangle)-1) & R2D_WORD_POSITION_MASK) ;
	end_src_dy=((r2d_get_ymax(src_rectangle)-1) & R2D_WORD_POSITION_MASK) ;
	
    //printf("dst_dy, end_dst_dy before reverse=%d,%d\n",dst_dy,end_dst_dy);
    // Number of rows is word containing last point - word containing first point
    // + 1

    // We remove - 1 because last PIXEL is at 1 from the frontier line
    dst_nb_rows=R2D_ALIGNED_MWLENGTH(r2d_get_ymax(dst_rectangle)-1)-R2D_ALIGNED_MWLENGTH(dst_y)+1;
    src_nb_rows=R2D_ALIGNED_MWLENGTH(r2d_get_ymax(src_rectangle)-1)-R2D_ALIGNED_MWLENGTH(src_y)+1;

    //printf("src_nb_rows %d\n",src_nb_rows);
    //printf("dst_nb_rows %d\n",dst_nb_rows);

    
    h_direction=1;
    v_direction=1;
    if (((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))==
       ((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer)))
      r2d_check_rectangle_overlap(src_x,src_y,dst_x,dst_y,
      rect_width,rect_height,&h_direction,&v_direction);

    //printf("h,v=%d,%d\n",h_direction,v_direction);


#if (R2D_REFRESH == R2D_VERTICAL)      
      src_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))->height;
      dst_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->height;
#else
	  src_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))->width;
      dst_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->width;
#endif

	  src_offset=R2D_ALIGNED_MWLENGTH(src_height);
      dst_offset=R2D_ALIGNED_MWLENGTH(dst_height);

      //printf("%08X,dst offset=%08X\n",p_dst,dst_offset);

      p_src_start=p_src
        +((src_x*src_offset+(src_y>>R2D_PIXELS_PER_MEMORY_WORD)));
      p_dst_start=p_dst
        +((dst_x*dst_offset+(dst_y>>R2D_PIXELS_PER_MEMORY_WORD)));

      xnb=rect_width;

      //printf("src start %08X contains %08X\n",p_src_start,*p_src_start);
      //printf("dst start %08X contains %08X\n",p_dst_start,*p_dst_start);

      //printf("rect_width %d\n",rect_width);
      if (h_direction==-1)
      {
		x=r2d_get_xmax(dst_rectangle)-1;
        p_src_start+=(rect_width-1)*src_offset;
        p_dst_start+=(rect_width-1)*dst_offset;
      }
	  else
         x=r2d_get_xmin(dst_rectangle);

      //printf("src start %08X contains %08X\n",p_src_start,*p_src_start);
      //printf("dst start %08X contains %08X\n",p_dst_start,*p_dst_start);

      if (v_direction==-1)
      {
        p_src_start+=src_nb_rows-1;
        p_dst_start+=dst_nb_rows-1;

        temp=src_dy;
        src_dy=end_src_dy;
        end_src_dy=temp;

        temp=dst_dy;
        dst_dy=end_dst_dy;
        end_dst_dy=temp;
      }


      //printf("src start %08X contains %08X\n",p_src_start,*p_src_start);
      //printf("dst start %08X contains %08X\n",p_dst_start,*p_dst_start);

      //printf("dst_start=%08X contains %08X\n",p_dst_start,*p_dst_start);
      while(xnb!=0)
      {
        p_dst_current=p_dst_start;
        p_src_current=p_src_start;

        //printf("xnb=%d\n",xnb);
        //printf("src start %08X contains %08X\n",p_src_start,*p_src_start);
        //printf("dst start %08X contains %08X\n",p_dst_start,*p_dst_start);


        ynb=dst_nb_rows-1;
        
        if (v_direction==-1)
        {
		   y=r2d_get_ymax(dst_rectangle) - 1;
           current_src_dy=(1<<R2D_PIXELS_PER_MEMORY_WORD)-1;
           current_dst_dy=(1<<R2D_PIXELS_PER_MEMORY_WORD)-1;
        }
        else
        {
           current_src_dy=0;
           current_dst_dy=0;
		   y=r2d_get_ymin(dst_rectangle);
        }
        
        dstcounter=(1<<R2D_PIXELS_PER_MEMORY_WORD);
        srccounter=(1<<R2D_PIXELS_PER_MEMORY_WORD);
        
        dst_current=*p_dst_current;
        src_current=*p_src_current;
	
        //printf("src_current=%08X, dst_current=%08X\n",src_current,dst_current);
        temp=0;
        //printf("new column\n dst=%08X\n",p_dst_current);
        //printf("current_dst_dy =%d, dst_dy=%d\n",current_dst_dy,dst_dy);
        dstvalue=0;
		if (v_direction==-1)
		{
			while(current_dst_dy!=dst_dy)
			{
                r2d_shift_pixel_up(dstvalue,dst_current,dstcounter,p_dst_current);
                current_dst_dy--;
                //printf("dstcounter=%d\n",dstcounter);
			    y--;
			}
			while(current_src_dy!=src_dy)
			{
            
              //printf("current_src_dy=%d\n",current_src_dy);
               //printf("srccounter=%d\n",srccounter);

              r2d_shift_pixel_up(temp,src_current,srccounter,p_src_current);
              current_src_dy--;
              //printf("src_current=%08X\n",src_current);
			  
            
			}
			while(ynb!=0)
			{
            
              r2d_shift_pixel_up_and_write(dst,src);
              //printf("srccounter=%d\n",srccounter);
			  y--;
            
              if (dstcounter==(1<<R2D_PIXELS_PER_MEMORY_WORD))
			  {
               ynb--;
               
			   *p_dst_current--=dstvalue;

#if (R2D_DEBUG == R2D_ON)
#if (R2D_DEBUG_WARNING == R2D_DEBUG_HIGH)
         if ((p_dst_current<((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->p_memory_words) 
         || (p_dst_current>((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->p_frame_buffer_end))
         {
              //printf("Error : frame_buffer overflow\n");
			  IND_rvf_send_trace("R2D : Framebuffer overflow1",27, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );
         }
#endif
#endif

               current_dst_dy=(1<<R2D_PIXELS_PER_MEMORY_WORD)-1;
               
               dst_current=*p_dst_current;  
                    
               //printf("%08X <- %08X\n",p_dst_current,dstvalue);

               dstvalue=0;
			  }
            
			}
			current_dst_dy++;
			while(current_dst_dy!=end_dst_dy)
			{
            
              r2d_shift_pixel_up_and_write(dst,src);
              current_dst_dy--;
              //printf("Last row %08X, %08X\n",p_dst_current,current_dst_dy);
			  y--;
            
			}
			if (dstcounter==(1<<R2D_PIXELS_PER_MEMORY_WORD))
               compensate=FALSE;
            while(dstcounter!=(1<<R2D_PIXELS_PER_MEMORY_WORD))
			{
             
                 r2d_shift_pixel_up(dstvalue,dst_current,dstcounter,p_dst_current);
                 current_dst_dy--;
                 //printf("   Last row %08X\n",p_src_current);
                 //printf("dstcounter=%d\n",dstcounter);
                 y--;

             

			}
			if (compensate==TRUE)
			{
			 *++p_dst_current=dstvalue;
          
			}
            else
			{
			 *p_dst_current=dstvalue;
           
			}
#if (R2D_DEBUG == R2D_ON)
#if (R2D_DEBUG_WARNING == R2D_DEBUG_HIGH)
         if ((p_dst_current<((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->p_memory_words) 
         || (p_dst_current>((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->p_frame_buffer_end))
         {
              //printf("Error : frame_buffer overflow\n");
			  IND_rvf_send_trace("R2D : Framebuffer overflow2",27, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );
         }
#endif
#endif

        
		}
		else
		{
			while(current_dst_dy!=dst_dy)
			{
              r2d_shift_pixel_down(dstvalue,dst_current,dstcounter,p_dst_current);
              current_dst_dy++;
			  y++;
              //printf("dstcounter=%d\n",dstcounter);
            }
			while(current_src_dy!=src_dy)
			{
            
              r2d_shift_pixel_down(temp,src_current,srccounter,p_src_current);
              current_src_dy++;
			  
            
			}
			while(ynb!=0)
			{
            
              r2d_shift_pixel_down_and_write(dst,src);
			  y++;
            
              if (dstcounter==(1<<R2D_PIXELS_PER_MEMORY_WORD))
			  {
                 ynb--;
               
        
               *p_dst_current++=dstvalue;
#if (R2D_DEBUG == R2D_ON)
#if (R2D_DEBUG_WARNING == R2D_DEBUG_HIGH)
         if ((p_dst_current<((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->p_memory_words) 
         || (p_dst_current>((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->p_frame_buffer_end))
         {
			 char error[256];
			 T_R2D_RECT *dst;

			 //dst=(T_R2D_RECT*)dst_rectangle;
			 //sprintf(error,"%d %d %d %d",dst->ul_x,dst->ul_y,
			//	 dst->br_x,dst->br_y);
			 //IND_rvf_send_trace(error,strlen(error), NULL_PARAM, 
			//	   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );
              //printf("Error : frame_buffer overflow\n");
			  IND_rvf_send_trace("R2D : Framebuffer overflow3",27, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );
         }
#endif
#endif

                 current_dst_dy=0;
               
                 dst_current=*p_dst_current;  
                    
               //printf("%08X <- %08X\n",p_dst_current,dstvalue);

                 dstvalue=0;
			  }
            
			}
			
            current_dst_dy--;
			while(current_dst_dy!=end_dst_dy)
			{
            
              r2d_shift_pixel_down_and_write(dst,src);
              current_dst_dy++;
              //printf("Last row %08X\n",p_dst_current);
			  y++;
            
			}
			if (dstcounter==(1<<R2D_PIXELS_PER_MEMORY_WORD))
              compensate=FALSE;
            while(dstcounter!=(1<<R2D_PIXELS_PER_MEMORY_WORD))
			{
             
                 r2d_shift_pixel_down(dstvalue,dst_current,dstcounter,p_dst_current);
                 current_dst_dy++;
                 //printf("Last row %08X\n",p_dst_current);
				 y++;
             
			}
			if (compensate==TRUE)
			{
				*--p_dst_current=dstvalue;
			}
            else
			{
				*p_dst_current=dstvalue;
			}
#if (R2D_DEBUG == R2D_ON)
#if (R2D_DEBUG_WARNING == R2D_DEBUG_HIGH)
         if ((p_dst_current<((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->p_memory_words) 
         || (p_dst_current>((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->p_frame_buffer_end))
         {
              //printf("Error : frame_buffer overflow\n");
			  IND_rvf_send_trace("R2D : Framebuffer overflow4",27, NULL_PARAM, 
				   RV_TRACE_LEVEL_ERROR, R2D_USE_ID );
         }
#endif
#endif

		}

        
        dstvalue=0;
        
          
        if (h_direction==-1)
        {
          p_src_start-=src_offset;
          p_dst_start-=dst_offset;
		  x--;
        }
        else
        {
          p_src_start+=src_offset;
          p_dst_start+=dst_offset;
		  x++;
        }

        xnb--;
      }

      
      
    
  }
end_blit: dstvalue=0;
}


#undef r2d_shift_pixel_down_and_write
#undef r2d_shift_pixel_up_and_write

#define r2d_shift_pixel_down_and_write(dst,src) {\
               dst##value =R2D_PIXEL_DOWN_OUT(dst##value); \
               dst##value|=dop((dst##_current & R2D_PIXEL_MASK), \
               r2d_convert_from_color_to_color(use_foreground_color,\
			   src##_current & R2D_PIXEL_MASK,src_gc,dst_gc,x,y)) \
               << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH) ; \
               src##_current=R2D_PIXEL_DOWN_OUT(src##_current); \
               dst##_current=R2D_PIXEL_DOWN_OUT(dst##_current); \
               dst##counter--; \
               src##counter--; \
               if (src##counter==0) \
               { \
                 src##counter=(1<<R2D_PIXELS_PER_MEMORY_WORD); \
                 src##_current=*++p_##src##_current;  \
               } \
               if (dst##counter==0) \
               { \
                 dst##counter=(1<<R2D_PIXELS_PER_MEMORY_WORD); \
               } \
               }

#define r2d_shift_pixel_up_and_write(dst,src) {\
               dst##value =R2D_PIXEL_UP_OUT(dst##value); \
               dst##value |=dop( \
               (dst##_current >> ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) & R2D_PIXEL_MASK, \
               r2d_convert_from_color_to_color(use_foreground_color,\
			   (src##_current >> ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) & R2D_PIXEL_MASK, \
			   src_gc,dst_gc,x,y)); \
               src##_current=R2D_PIXEL_UP_OUT(src##_current); \
               dst##_current=R2D_PIXEL_UP_OUT(dst##_current); \
               dst##counter--; \
               src##counter--; \
               if (src##counter==0) \
               { \
                 src##counter=(1<<R2D_PIXELS_PER_MEMORY_WORD); \
                 src##_current=*--p_##src##_current;  \
               } \
               if (dst##counter==0) \
               { \
                 dst##counter=(1<<R2D_PIXELS_PER_MEMORY_WORD); \
               } \
               }



// Requires shapes in graphic context coordinates
void           r2d_blit_color_to_color(T_R2D_GC_PTR src_gc,T_R2D_GC_PTR dst_gc,
                             T_R2D_SHAPE_PTR src_rectangle, 
							 T_R2D_SHAPE_PTR dst_rectangle,
							 R2D_BOOLEAN use_foreground_color)
{
    UINT32 *p_src,*p_dst,*p_src_current,*p_dst_current,*p_src_start,*p_dst_start,value;
    INT16 rect_width,rect_height,src_x,src_y,dst_x,dst_y;
    INT16 src_offset,dst_offset,src_height,dst_height;
    INT16 xnb,ynb,nb_rows;
    INT16 x,y;//,temp;


    INT32  h_direction,v_direction;
    T_R2D_DRAWING_MODE mode;
    T_R2D_DRAWING_OP dop;
	

    dop=((T_R2D_GC*)dst_gc)->drawing_op;

    

    {

    p_src=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))->p_memory_words;
    p_dst=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->p_memory_words;

    rect_width=r2d_get_xmax(src_rectangle)-r2d_get_xmin(src_rectangle);
    rect_height=r2d_get_ymax(src_rectangle)-r2d_get_ymin(src_rectangle);

	if ((rect_width<=0) || (rect_height<=0))
		goto end_blit_c;

    mode=IND_r2d_get_drawing_mode(dst_gc);

    
    src_x=r2d_get_xmin(src_rectangle);
    src_y=r2d_get_ymin(src_rectangle);

    dst_x=r2d_get_xmin(dst_rectangle);
    dst_y=r2d_get_ymin(dst_rectangle);


    // Number of rows is word containing last point - word containing first point
    // + 1
    nb_rows=(r2d_get_ymax(dst_rectangle)-1)-(dst_y)+1;
    //printf("nb_rows %d\n",nb_rows);
    
    h_direction=1;
    v_direction=1;
    if (((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))==
       ((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer)))
      r2d_check_rectangle_overlap(src_x,src_y,dst_x,dst_y,
      rect_width,rect_height,&h_direction,&v_direction);

    //printf("h,v=%d,%d\n",h_direction,v_direction);


 #if (R2D_REFRESH == R2D_VERTICAL)      
      src_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))->height;
      dst_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->height;
#else
	  src_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))->width;
      dst_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->width;
#endif

      src_offset=(src_height);
      dst_offset=(dst_height);

      //printf("%08X,dst offset=%08X\n",p_dst,dst_offset);

      p_src_start=p_src
        +((src_x*src_offset+(src_y)));
      p_dst_start=p_dst
        +((dst_x*dst_offset+(dst_y)));

      xnb=rect_width;

      //printf("start %08X contains %08X\n",p_dst_start,*p_dst_start);

      if (h_direction==-1)
      {
		x=r2d_get_xmax(dst_rectangle)-1;
        p_src_start+=(rect_width-1)*src_offset;
        p_dst_start+=(rect_width-1)*dst_offset;
      }
	  else
		x=r2d_get_xmin(dst_rectangle);

      if (v_direction==-1)
      {
        p_src_start+=nb_rows-1;
        p_dst_start+=nb_rows-1;

      }

      //printf("dst_start=%08X contains %08X\n",p_dst_start,*p_dst_start);
      while(xnb!=0)
      {
        p_dst_current=p_dst_start;
        p_src_current=p_src_start;

        ynb=nb_rows; // Not using - 1 since not different processing of ending word
        // (which is here a pixel instead of being only a memory word)
        
        
        
        //printf("src_current=%08X, dst_current=%08X\n",src_current,dst_current);
        //printf("new column\n dst=%08X\n",p_dst_current);
        
		if (v_direction==-1)
		{
			while(ynb!=0)
			{
            
              value=*p_dst_current;
              *p_dst_current--=dop(value,*p_src_current--);
			  y--;
            
              ynb--;
			}
		}
		else
		{
			while(ynb!=0)
			{
            
              value=*p_dst_current;
              *p_dst_current++=dop(value,*p_src_current++);
			  y++;
           
              ynb--;
			}
		}

        
          
        if (h_direction==-1)
        {
          p_src_start-=src_offset;
          p_dst_start-=dst_offset;
		  x--;
        }
        else
        {
          p_src_start+=src_offset;
          p_dst_start+=dst_offset;
		  x++;
        }

        xnb--;
      }

      
      
   
  }
end_blit_c:ynb=0; 
}


#undef r2d_shift_pixel_down_and_write
#undef r2d_shift_pixel_up_and_write

#define r2d_shift_pixel_down_and_write(dst,src) {\
               dst##value =r2d_convert_from_lcd_to_color(use_foreground_color,\
			   src##_current & R2D_PIXEL_MASK,src_gc,dst_gc,x,y); \
               dst##value =dop(*p_dst_current,dst##value); \
               *p_##dst##_current++=dst##value;\
               src##_current=R2D_PIXEL_DOWN_OUT(src##_current); \
               src##counter--; \
               if (src##counter==0) \
               { \
                 src##counter=(1<<R2D_PIXELS_PER_MEMORY_WORD); \
                 src##_current=*++p_##src##_current;  \
               } \
               }


#define r2d_shift_pixel_up_and_write(dst,src) {\
               dst##value =r2d_convert_from_lcd_to_color(use_foreground_color,\
			   src##_current & R2D_PIXEL_MASK,src_gc,dst_gc,x,y); \
               dst##value =dop(*p_dst_current,dst##value); \
               *p_##dst##_current--=dst##value;\
               src##_current=R2D_PIXEL_UP_OUT(src##_current); \
               src##counter--; \
               if (src##counter==0) \
               { \
                 src##counter=(1<<R2D_PIXELS_PER_MEMORY_WORD); \
                 src##_current=*--p_##src##_current;  \
               } \
               }

// Requires shapes in graphic context coordinates
void           r2d_blit_lcd_to_color(T_R2D_GC_PTR src_gc,T_R2D_GC_PTR dst_gc,
                             T_R2D_SHAPE_PTR src_rectangle, 
							 T_R2D_SHAPE_PTR dst_rectangle,
							 R2D_BOOLEAN use_foreground_color)
{
    UINT32 *p_src,*p_dst,*p_src_current,*p_dst_current,*p_src_start,*p_dst_start;
    INT16 rect_width,rect_height,src_x,src_y,dst_x,dst_y;
    INT16 src_offset,dst_offset,src_height,dst_height;
    INT16 xnb,ynb,src_nb_rows,dst_nb_rows;
    INT16 src_dy,dst_dy,end_dst_dy,end_src_dy; // distance from word boundary
//    INT16 shift; // relative position (modulo a memory word) between both rectangles
    // in bits
    INT16 current_src_dy;//,current_dst_dy;
    INT16 temp,srccounter;//,dstcounter;


    register UINT32 dstvalue;//,dst_current;
    register UINT32 src_current;//,srcvalue;
    INT32  h_direction,v_direction;
	INT16 x,y;
    T_R2D_DRAWING_MODE mode;
	T_R2D_DRAWING_OP dop;

    dop=((T_R2D_GC*)dst_gc)->drawing_op;


    

    {

    p_src=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))->p_memory_words;
    p_dst=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->p_memory_words;

    rect_width=r2d_get_xmax(src_rectangle)-r2d_get_xmin(src_rectangle);
    rect_height=r2d_get_ymax(src_rectangle)-r2d_get_ymin(src_rectangle);

	if ((rect_width<=0) || (rect_height<=0))
		goto end_blit_d;

    mode=IND_r2d_get_drawing_mode(dst_gc);

    
    src_x=r2d_get_xmin(src_rectangle);
    src_y=r2d_get_ymin(src_rectangle);

    dst_x=r2d_get_xmin(dst_rectangle);
    dst_y=r2d_get_ymin(dst_rectangle);

    src_dy=src_y & R2D_WORD_POSITION_MASK;
	dst_dy=dst_y & R2D_WORD_POSITION_MASK;
	
    // Clipping convention such end_dst_dy is the first y position which must not
    // be copied at the end of a column
    end_dst_dy=(r2d_get_ymax(dst_rectangle)-1) & R2D_WORD_POSITION_MASK;
	end_src_dy=(r2d_get_ymax(src_rectangle)-1) & R2D_WORD_POSITION_MASK ;
	
    // Number of rows is word containing last point - word containing first point
    // + 1
    src_nb_rows=R2D_ALIGNED_MWLENGTH(r2d_get_ymax(src_rectangle)-1)-R2D_ALIGNED_MWLENGTH(src_y) + 1;
    
    dst_nb_rows=(r2d_get_ymax(dst_rectangle))-(dst_y);
    //printf("nb_rows %d\n",nb_rows);
    
    h_direction=1;
    v_direction=1;
    if (((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))==
       ((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer)))
      r2d_check_rectangle_overlap(src_x,src_y,dst_x,dst_y,
      rect_width,rect_height,&h_direction,&v_direction);

    //printf("h,v=%d,%d\n",h_direction,v_direction);


      
#if (R2D_REFRESH == R2D_VERTICAL)      
      src_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))->height;
      dst_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->height;
#else
	  src_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))->width;
      dst_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->width;
#endif
   
      src_offset=R2D_ALIGNED_MWLENGTH(src_height);
      dst_offset=dst_height;

      //printf("%08X,dst offset=%08X\n",p_dst,dst_offset);

      p_src_start=p_src
        +((src_x*src_offset+(src_y>>R2D_PIXELS_PER_MEMORY_WORD)));
      p_dst_start=p_dst
        +((dst_x*dst_offset+(dst_y)));

      xnb=rect_width;

      //printf("start %08X contains %08X\n",p_dst_start,*p_dst_start);

      if (h_direction==-1)
      {
		x=r2d_get_xmax(dst_rectangle)-1;
        p_src_start+=(rect_width-1)*src_offset;
        p_dst_start+=(rect_width-1)*dst_offset;
      }
	  else
		  x=r2d_get_xmin(dst_rectangle);

      if (v_direction==-1)
      {
        p_src_start+=src_nb_rows-1;
        p_dst_start+=dst_nb_rows-1;

        temp=src_dy;
        src_dy=end_src_dy;
        end_src_dy=temp;

      }

      //printf("dst_start=%08X contains %08X\n",p_dst_start,*p_dst_start);
      while(xnb!=0)
      {
        p_dst_current=p_dst_start;
        p_src_current=p_src_start;

        ynb=dst_nb_rows; // No -1 since no different processing for last word
        
        if (v_direction==-1)
        {
           current_src_dy=(1<<R2D_PIXELS_PER_MEMORY_WORD)-1;
		   y=r2d_get_ymax(dst_rectangle)-1;
        }
        else
        {
           current_src_dy=0;
		   y=r2d_get_ymin(dst_rectangle);
        }
        
        srccounter=(1<<R2D_PIXELS_PER_MEMORY_WORD);
        
        src_current=*p_src_current;
        //printf("src_current=%08X, dst_current=%08X\n",src_current,dst_current);
        temp=0;
        //printf("new column\n dst=%08X\n",p_dst_current);
        
        //printf("src_dy=%d\n",src_dy);

		if (v_direction==-1)
		{
			while(current_src_dy!=src_dy)
			{
            
              //printf("current_src_dy=%d\n",current_src_dy);
              r2d_shift_pixel_up(temp,src_current,srccounter,p_src_current);
              current_src_dy--;
			  
              //printf("src_current=%08X\n",src_current);
            
			}
			//printf("src_current=%08X, dst_current=%08X, dstvalue=%08X\n dstcounter=%08X, srccounter=%08X\n",
        //src_current,dst_current,dstvalue,dstcounter,srccounter);
            while(ynb!=0)
			{
            
              r2d_shift_pixel_up_and_write(dst,src);
			  y--;
            
              ynb--;

			}
		}
		else
		{
			while(current_src_dy!=src_dy)
			{
            
              r2d_shift_pixel_down(temp,src_current,srccounter,p_src_current);
              current_src_dy++;
			  
            
			}
			//printf("src_current=%08X, dst_current=%08X, dstvalue=%08X\n dstcounter=%08X, srccounter=%08X\n",
        //src_current,dst_current,dstvalue,dstcounter,srccounter);
           while(ynb!=0)
		   {
            
              r2d_shift_pixel_down_and_write(dst,src);
			  y++;
            
            ynb--;

		   }
		}

        
        
        
          
        if (h_direction==-1)
        {
          p_src_start-=src_offset;
          p_dst_start-=dst_offset;
		  x--;
        }
        else
        {
          p_src_start+=src_offset;
          p_dst_start+=dst_offset;
		  x++;
        }

        xnb--;
      }

      
      
    
  }
end_blit_d:ynb=0;
}



#undef r2d_shift_pixel_down_and_write
#undef r2d_shift_pixel_up_and_write

#define r2d_shift_pixel_down_and_write(dst,src) {\
               dst##value =R2D_PIXEL_DOWN_OUT(dst##value); \
               src##_current=r2d_convert_from_color_to_lcd(use_foreground_color,\
			   *p_##src##_current++,src_gc,dst_gc,x,y); \
               dst##value|=dop((dst##_current & R2D_PIXEL_MASK), \
               src##_current) \
               << ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH) ; \
               dst##_current=R2D_PIXEL_DOWN_OUT(dst##_current); \
               dst##counter--; \
               if (dst##counter==0) \
               { \
                 dst##counter=(1<<R2D_PIXELS_PER_MEMORY_WORD); \
               } \
               }


#define r2d_shift_pixel_up_and_write(dst,src) {\
               dst##value =R2D_PIXEL_UP_OUT(dst##value); \
               src##_current=r2d_convert_from_color_to_lcd(use_foreground_color,\
			   *p_##src##_current--,src_gc,dst_gc,x,y); \
               dst##value |=dop( \
               (dst##_current >> ((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)) & R2D_PIXEL_MASK, \
               src##_current); \
               dst##_current=R2D_PIXEL_UP_OUT(dst##_current); \
               dst##counter--; \
               if (dst##counter==0) \
               { \
                 dst##counter=(1<<R2D_PIXELS_PER_MEMORY_WORD); \
               } \
               }

// Requires shapes in graphic context coordinates
void           r2d_blit_color_to_lcd(T_R2D_GC_PTR src_gc,T_R2D_GC_PTR dst_gc,
                             T_R2D_SHAPE_PTR src_rectangle, 
							 T_R2D_SHAPE_PTR dst_rectangle,
							 R2D_BOOLEAN use_foreground_color)
{
    UINT32 *p_src,*p_dst,*p_src_current,*p_dst_current,*p_src_start,*p_dst_start;
    INT16 rect_width,rect_height,src_x,src_y,dst_x,dst_y;
    INT16 src_offset,dst_offset,src_height,dst_height;
    INT16 xnb,ynb,dst_nb_rows,src_nb_rows;
    INT16 src_dy,dst_dy,end_dst_dy,end_src_dy; // distance from word boundary
//    INT16 shift; // relative position (modulo a memory word) between both rectangles
    // in bits
    INT16 current_dst_dy;//,current_src_dy;
    INT16 temp,dstcounter,srccounter;


    register UINT32 dstvalue,dst_current;
    register UINT32 src_current;//,srcvalue;
    INT32  h_direction,v_direction;
	INT16 x,y;
    T_R2D_DRAWING_MODE mode;
    T_R2D_DRAWING_OP dop;
	BOOLEAN compensate=TRUE;

    dop=((T_R2D_GC*)dst_gc)->drawing_op;


    

     {

    p_src=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))->p_memory_words;
    p_dst=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->p_memory_words;

    rect_width=r2d_get_xmax(src_rectangle)-r2d_get_xmin(src_rectangle);
    rect_height=r2d_get_ymax(src_rectangle)-r2d_get_ymin(src_rectangle);

	if ((rect_width<=0) || (rect_height<=0))
		goto end_blit_e;

    mode=IND_r2d_get_drawing_mode(dst_gc);

    
    src_x=r2d_get_xmin(src_rectangle);
    src_y=r2d_get_ymin(src_rectangle);

    dst_x=r2d_get_xmin(dst_rectangle);
    dst_y=r2d_get_ymin(dst_rectangle);

    src_dy=src_y & R2D_WORD_POSITION_MASK;
	dst_dy=dst_y & R2D_WORD_POSITION_MASK;
	
    // Clipping convention such end_dst_dy is the first y position which must not
    // be copied at the end of a column
    end_dst_dy=(r2d_get_ymax(dst_rectangle)-1) & R2D_WORD_POSITION_MASK ;
	end_src_dy=(r2d_get_ymax(src_rectangle)-1) & R2D_WORD_POSITION_MASK ;
	
    // Number of rows is word containing last point - word containing first point
    // + 1
    dst_nb_rows=R2D_ALIGNED_MWLENGTH(r2d_get_ymax(dst_rectangle)-1)-R2D_ALIGNED_MWLENGTH(dst_y) + 1;
    src_nb_rows=(r2d_get_ymax(src_rectangle))-(src_y);

    //printf("src_nb_rows %d\n",src_nb_rows);
    //printf("dst_nb_rows %d\n",dst_nb_rows);
    
    h_direction=1;
    v_direction=1;
    if (((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))==
       ((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer)))
      r2d_check_rectangle_overlap(src_x,src_y,dst_x,dst_y,
      rect_width,rect_height,&h_direction,&v_direction);

    //printf("h,v=%d,%d\n",h_direction,v_direction);



       
#if (R2D_REFRESH == R2D_VERTICAL)      
      src_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))->height;
      dst_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->height;
#else
	  src_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)src_gc)->p_frame_buffer))->width;
      dst_height=((T_R2D_FRAMEBUFFER*)(((T_R2D_GC*)dst_gc)->p_frame_buffer))->width;
#endif
        
      src_offset=src_height;
      dst_offset=R2D_ALIGNED_MWLENGTH(dst_height);

      //printf("%08X,dst offset=%08X\n",p_dst,dst_offset);

      p_src_start=p_src
        +((src_x*src_offset+(src_y)));
      p_dst_start=p_dst
        +((dst_x*dst_offset+(dst_y>>R2D_PIXELS_PER_MEMORY_WORD)));

      xnb=rect_width;

      //printf("start %08X contains %08X\n",p_dst_start,*p_dst_start);

      if (h_direction==-1)
      {
        x=r2d_get_xmax(dst_rectangle)-1;
        p_src_start+=(rect_width-1)*src_offset;
        p_dst_start+=(rect_width-1)*dst_offset;
      }
      else
        x=r2d_get_xmin(dst_rectangle);

      if (v_direction==-1)
      {
        p_src_start+=src_nb_rows-1;
        p_dst_start+=dst_nb_rows-1;

        temp=dst_dy;
        dst_dy=end_dst_dy;
        end_dst_dy=temp;
      }
      

      //printf("dst_start=%08X contains %08X\n",p_dst_start,*p_dst_start);
      while(xnb!=0)
      {
        p_dst_current=p_dst_start;
        p_src_current=p_src_start;

        ynb=dst_nb_rows-1;
        
        if (v_direction==-1)
        {
           y=r2d_get_ymax(dst_rectangle) - 1;
           current_dst_dy=(1<<R2D_PIXELS_PER_MEMORY_WORD)-1;
        }
        else
        {
           current_dst_dy=0;
           y=r2d_get_ymin(dst_rectangle);
        }
        
        dstcounter=(1<<R2D_PIXELS_PER_MEMORY_WORD);
        srccounter=(1<<R2D_PIXELS_PER_MEMORY_WORD);
        
        dst_current=*p_dst_current;
        //printf("src_current=%08X, dst_current=%08X\n",src_current,dst_current);
        temp=0;
        //printf("new column\n dst=%08X\n",p_dst_current);
        dstvalue=0;
        //printf("current_dst_dy=%08X,dst_dy=%08X\n",current_dst_dy,dst_dy);

		if (v_direction==-1)
		{
			 while(current_dst_dy!=dst_dy)
			 {
            
              r2d_shift_pixel_up(dstvalue,dst_current,dstcounter,p_dst_current);
              current_dst_dy--;
              y--;
              //printf("dstcounter=%d\n",dstcounter);
            
			 }
			 while(ynb!=0)
			 {
            
              r2d_shift_pixel_up_and_write(dst,src);
              y--;
            
              if (dstcounter==(1<<R2D_PIXELS_PER_MEMORY_WORD))
			  {
                ynb--;
               
               
               
                 *p_dst_current--=dstvalue;
                  current_dst_dy=(1<<R2D_PIXELS_PER_MEMORY_WORD)-1;
               
                  dst_current=*p_dst_current;  
                    
               //printf("%08X <- %08X\n",p_dst_current,dstvalue);

                  dstvalue=0;
			  }
            
			 }
			    current_dst_dy++;

			while(current_dst_dy!=end_dst_dy)
			{
            
              r2d_shift_pixel_up_and_write(dst,src);
              current_dst_dy--;
              y--;
              //printf("Last row %08X, %08X\n",p_dst_current,current_dst_dy);
            
			}
			if (dstcounter==(1<<R2D_PIXELS_PER_MEMORY_WORD))
               compensate=FALSE;
           while(dstcounter!=(1<<R2D_PIXELS_PER_MEMORY_WORD))
		   {
             
                 r2d_shift_pixel_up(dstvalue,dst_current,dstcounter,p_dst_current);
                 current_dst_dy--;
                 //printf("Last row %08X\n",p_dst_current);
                 //printf("dstcounter=%d\n",dstcounter);
                 y--;

             

		   }
		   if (compensate==TRUE)
		   {
             *++p_dst_current=dstvalue;
            
		   }
           else
		   {
             *p_dst_current=dstvalue;
             
		   }
        
		}
		else
		{
			while(current_dst_dy!=dst_dy)
			{
            
              r2d_shift_pixel_down(dstvalue,dst_current,dstcounter,p_dst_current);
              current_dst_dy++;
              y++;
              //printf("dstcounter=%d\n",dstcounter);
            
            }
			while(ynb!=0)
			{
            
              r2d_shift_pixel_down_and_write(dst,src);
              y++;
            
              if (dstcounter==(1<<R2D_PIXELS_PER_MEMORY_WORD))
			  {
               ynb--;
               
               
               
               
                *p_dst_current++=dstvalue;
                current_dst_dy=0;
               
                dst_current=*p_dst_current;  
                    
               //printf("%08X <- %08X\n",p_dst_current,dstvalue);

                dstvalue=0;
			  }
            
			}
			current_dst_dy--;
        
           while(current_dst_dy!=end_dst_dy)
		   {
            
              r2d_shift_pixel_down_and_write(dst,src);
              current_dst_dy++;
              y++;
              //printf("Last row %08X\n",p_dst_current);
            
		   }
		   if (dstcounter==(1<<R2D_PIXELS_PER_MEMORY_WORD))
             compensate=FALSE;
          while(dstcounter!=(1<<R2D_PIXELS_PER_MEMORY_WORD))
		  {
             
                 r2d_shift_pixel_down(dstvalue,dst_current,dstcounter,p_dst_current);
                 current_dst_dy++;
                 //printf("Last row %08X\n",p_dst_current);
                 y++;
             

		  }
		   if (compensate==TRUE)
		  {
            *--p_dst_current=dstvalue;
		  }
          else
		  {
            *p_dst_current=dstvalue;
		  }
		}

        
        //printf("src_current=%08X, dst_current=%08X, dstvalue=%08X\n dstcounter=%08X, srccounter=%08X\n",
        //src_current,dst_current,dstvalue,dstcounter,srccounter);
        
        // One need a different processing for last word
        //printf("Last row %08X,end_dst_dy=%08X,current_dy=%08X\n",p_dst_current,end_dst_dy,current_dst_dy);
        
            
        //end_dst_dy MUST be copied
        // It is at 1 from the copy bit rectangle frontier
        
        
        
        //printf("dstvalue: %08X <- %08X\n",p_dst_current,dstvalue);
        
        dstvalue=0;
        
          
        if (h_direction==-1)
        {
          x--;
          p_src_start-=src_offset;
          p_dst_start-=dst_offset;
        }
        else
        {
          x++;
          p_src_start+=src_offset;
          p_dst_start+=dst_offset;
        }

        xnb--;
      }

      
      
    
  }
end_blit_e:dstvalue=0;
}

#endif

/*********************

  R2D ASM MODE

**********************/

#if (R2D_ASM == R2D_ON)

#define R2D_BRANCH_INST 0xFF000000

/*
Patch a block of 3 instructions with a block defined
by the user. The block does not contain any branch instruction.
The branch must use a special syntax introduced by the long word
R2D_BRANCH_INST
*/
void r2d_patch_code_block(UINT32* src,UINT32 *dst)
{
  

    if (*src == R2D_BRANCH_INST)
    {
       *dst++ = *(&r2d_start_branch_inst);
       src++;
       // PC is ahead of 8 bytes of current instruction because of prefetch
       // So -8 must be used
       *dst = (((((*src) - ((UINT32)dst)) - 8)>>2) & 0x0FFFFFF) | 0xEB000000;
       *++dst= *(&r2d_stop_branch_inst);
    }
    else
    {
       
       *dst++=*src++;
       *dst++=*src++;
       *dst++=*src++;
    }
}

/*
Copy one instruction of code
*/
void r2d_patch_inst(UINT32* src,UINT32 *dst)
{
  *dst++ = *src++;
}


/*
Patch the sequence of instructions reading the foreground 
background colors. It copies a sequence containing
two LDR and update the LDR offsets
*/
void r2d_patch_get_color(UINT32* src,UINT32 *dst)
{
       UINT32 r,offset;
       INT16 delta;

       delta=(((INT32)src-(INT32)dst));

       r=*src++;
       offset=r & 0xFFF;
       offset+=delta;
       offset=offset & 0xFFF;
       *dst++ = (r & 0xFFFFF000) | offset;
       
       r=*src++;
       offset=r & 0xFFF;
       offset+=delta;
       offset=offset & 0xFFF;
       *dst++ = (r & 0xFFFFF000) | offset;

      *dst++ = *src++;
}

/*
Patch with a branch if f is true or add a NOP
*/
void r2d_patch_branch(UINT32 *src,UINT32 *dst,BOOLEAN f)
{
  INT32 offset;
  if (f)
  {
     offset=(((((INT32)dst-(INT32)src))-8)>>2) & 0x00FFFFFF;
     *src=(0xEA<<24) | offset;
  }
  else
  {
     *src=0xe1a0b00b;
  }
}


/*
Replace the shift value by another one
*/
#define R2D_MOV_OPCODE 0xD
void r2d_patch_shift(UINT32 *src,UINT32 shift,INT16 left)
{
  UINT32 data;
  
  data=*src;


  // Shift addressing by default
  data=data  & 0xFDFFFFFF  ;

  
  if ((shift !=0) && (shift != 32))
  {
	 data=data & 0xFFFFF00F;
	 if (left==1)
        data=data|(((shift&0x01F)<<7)&0x0F80); // LSL
	 else
        data=data|(((shift&0x01F)<<7)&0x0F80)| 0x20; // LSR
  }
  else if (shift != 32)
  {
     data=data & 0xFFFFF00F;
     data=data|(((shift&0x01F)<<7)&0x0F80); // LSL 0
	
  }
  else
  {
     data=(data & 0xFFFFF07F); // LSR 0
	  
  }
  *src=data;
}

void r2d_patch_moveshift(UINT32 *src,UINT32 shift,INT16 left)
{
  UINT32 data;
  UINT32 dstreg;
  UINT32 opcode;

  data=*src;

  dstreg=(data & 0x0F000);
  dstreg=dstreg>>12;

  opcode=data & 0x01E00000;
  opcode=opcode>>21;
  if (opcode!=R2D_MOV_OPCODE)
	  dstreg=0;

  // Shift addressing by default
  data=data  & 0xF1FFFFFF  ;

  
  if ((shift !=0) && (shift != 32))
  {
	 if (opcode==R2D_MOV_OPCODE)
        data=data & 0xFFFFF000;
	 else
        data=data & 0xFFFFF00F;
	 if (left==1)
        data=data|(((shift&0x01F)<<7)&0x0F80)|dstreg; // LSL
	 else
        data=data|(((shift&0x01F)<<7)&0x0F80)| 0x20|dstreg; // LSR
  }
  else if (shift != 32)
  {
     if (opcode==R2D_MOV_OPCODE)
        data=data & 0xFFFFF000;
	 else
        data=data & 0xFFFFF00F;
     data=data|(((shift&0x01F)<<7)&0x0F80)|dstreg; // LSL 0
	
  }
  else
  {
     //data=(data & 0xFFFFF07F); // LSR 32
	  data=(data & 0xFFFFF000) | 0x02000000; // LSR 32
	  
  }
  *src=data;
}
/*
Patch the const table contained in the .text section
with the new foreground and background values
*/
void r2d_patch_color_conversion(T_R2D_GC_PTR gc)
{
  UINT32 *p;
  
  p=&r2d_blit_foreground;
  *p=((T_R2D_GC*)gc)->foreground_pixel_value;
  
  p=&r2d_blit_background;
  *p=((T_R2D_GC*)gc)->background_pixel_value;
  
}

void r2d_patch_blit(T_R2D_GC_PTR dst_gc,T_R2D_DRAWING_MODE mode,
					R2D_BOOLEAN use_foreground_color,INT32 srcdst)
{
	// Patch mode
	
extern R2D_BOOLEAN r2d_g_old_use_foreground;
extern INT32 r2d_g_old_srcdst;

    if (r2d_g_old_mode != mode)
	{
		r2d_g_old_mode=mode;
	    r2d_patch_code_block(r2d_get_asm_drawing_op(dst_gc,mode),&r2d_sb_down1);
        r2d_patch_code_block(r2d_get_asm_drawing_op(dst_gc,mode),&r2d_sb_down2);
        r2d_patch_code_block(r2d_get_asm_drawing_op(dst_gc,mode),&r2d_sa_up1);
        r2d_patch_code_block(r2d_get_asm_drawing_op(dst_gc,mode),&r2d_sa_up2);
	}

    // Meaning of variables names
    // Last word is [ab]_(down1|down2|up1|up2)
    // a or b is the blit version (blit_a oe blit_b)
    // down described which version of the macro is used
    // (down or up one) and the number is used to differentiate
    // the calls to the macro

    // For the prefix, sft_ is for part of the code where a shift must be
    // patched. For instance: sft_dst_n_a_up1
    // The second part says if the shift is for a src, dst or other
    // n means noraml shift like R2D_PIXEL_DEPTH
    // The n means complementd for shift as (1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH

	// Patch color detection
	if (r2d_g_old_use_foreground != use_foreground_color)
	{
		r2d_g_old_use_foreground=use_foreground_color;
        if (use_foreground_color)
		{
		    if (((T_R2D_FRAMEBUFFER*)(((T_R2D_GC *)dst_gc)->p_frame_buffer))->kind!=R2D_FULL_KIND)
			{
               r2d_patch_code_block(&r2d_lcd_start_foreground_pixel,&r2d_sdb_down1);
               r2d_patch_code_block(&r2d_lcd_start_foreground_pixel,&r2d_sdb_down2);
               r2d_patch_code_block(&r2d_lcd_start_foreground_pixel,&r2d_sda_up1);
               r2d_patch_code_block(&r2d_lcd_start_foreground_pixel,&r2d_sda_up2);
			}
		    else
			{
			   r2d_patch_code_block(&r2d_color_start_foreground_pixel,&r2d_sdb_down1);
               r2d_patch_code_block(&r2d_color_start_foreground_pixel,&r2d_sdb_down2);
               r2d_patch_code_block(&r2d_color_start_foreground_pixel,&r2d_sda_up1);
               r2d_patch_code_block(&r2d_color_start_foreground_pixel,&r2d_sda_up2);
			}

            r2d_patch_color_conversion(dst_gc);
			r2d_g_old_foreground_pixelvalue=((T_R2D_GC*)dst_gc)->foreground_pixel_value;
            r2d_g_old_background_pixelvalue=((T_R2D_GC*)dst_gc)->background_pixel_value;


            r2d_patch_get_color(&r2d_start_get_color,&r2d_scb_down1);
            r2d_patch_get_color(&r2d_start_get_color,&r2d_scb_down2);
            r2d_patch_get_color(&r2d_start_get_color,&r2d_sca_up1);
            r2d_patch_get_color(&r2d_start_get_color,&r2d_sca_up2);
		}
        else
		{
			
            r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_sdb_down1);
            r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_sdb_down2);
            r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_sda_up1);
            r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_sda_up2);

            r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_scb_down1);
            r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_scb_down2);
            r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_sca_up1);
            r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_sca_up2);
		}
	}
	else
	{
		if (use_foreground_color)
			{
		       if ((r2d_g_old_foreground_pixelvalue != ((T_R2D_GC*)dst_gc)->foreground_pixel_value)
			      || 
		          (r2d_g_old_background_pixelvalue != ((T_R2D_GC*)dst_gc)->background_pixel_value))
			   {
                    r2d_patch_color_conversion(dst_gc);
					r2d_g_old_foreground_pixelvalue=((T_R2D_GC*)dst_gc)->foreground_pixel_value;
                    r2d_g_old_background_pixelvalue=((T_R2D_GC*)dst_gc)->background_pixel_value;
			   }
		
			}
	}

	if (r2d_g_old_srcdst != srcdst)
	{
		r2d_g_old_srcdst=srcdst;

	    // Path color conversion
        if (srcdst==R2D_LCDLCD)
		{
           r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_sconvcolb_down1);
           r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_sconvcolb_down2);
           r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_sconvcola_up1);
           r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_sconvcola_up2);

		   // write_shift_pixel_down
		   r2d_patch_branch(&r2d_skip_load_src_a_up1,&r2d_no_null_src_counter_a,FALSE);//src
		   r2d_patch_branch(&r2d_skip_load_src_a_up2,&r2d_end_a_up2,FALSE);//src

		   // write_shift_pixel_up
		   r2d_patch_branch(&r2d_skip_load_src_b_down1,&r2d_no_null_src_counter_b,FALSE);//src
		   r2d_patch_branch(&r2d_skip_load_src_b_down2,&r2d_end_b_down2,FALSE);//src

		   

		   r2d_patch_branch(&r2d_skip_shift_down_downa,&r2d_end_downa,FALSE);// dst
		   r2d_patch_branch(&r2d_skip_shift_down_downb,&r2d_end_downb,FALSE);//src
		   r2d_patch_branch(&r2d_skip_shift_down_downb,&r2d_end_downc,FALSE);//dst

		   r2d_patch_branch(&r2d_skip_shift_up_upa,&r2d_end_upa,FALSE);//dst
		   r2d_patch_branch(&r2d_skip_shift_up_upb,&r2d_end_upb,FALSE);//src
		   r2d_patch_branch(&r2d_skip_shift_up_upc,&r2d_end_upc,FALSE);//dst

		   r2d_patch_inst(&r2d_never_write,&r2d_always_read_b_down1);
		   r2d_patch_inst(&r2d_never_write,&r2d_always_read_b_down2);
		   r2d_patch_inst(&r2d_never_write,&r2d_always_read_a_up1);
		   r2d_patch_inst(&r2d_never_write,&r2d_always_read_a_up2);


		}

		if (srcdst==R2D_COLORCOLOR)
		{
           r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_sconvcolb_down1);
           r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_sconvcolb_down2);
           r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_sconvcola_up1);
           r2d_patch_code_block(&r2d_lcd_start_nothing,&r2d_sconvcola_up2);

		   // write_shift_pixel_down
		   r2d_patch_branch(&r2d_skip_load_src_a_up1,&r2d_no_null_src_counter_a,TRUE);//src
		   r2d_patch_branch(&r2d_skip_load_src_a_up2,&r2d_end_a_up2,TRUE);//src

		   // write_shift_pixel_up
		   r2d_patch_branch(&r2d_skip_load_src_b_down1,&r2d_no_null_src_counter_b,TRUE);//src
		   r2d_patch_branch(&r2d_skip_load_src_b_down2,&r2d_end_b_down2,TRUE);//src

		   

		   r2d_patch_branch(&r2d_skip_shift_down_downa,&r2d_end_downa,TRUE);// dst
		   r2d_patch_branch(&r2d_skip_shift_down_downb,&r2d_end_downb,TRUE);//src
		   r2d_patch_branch(&r2d_skip_shift_down_downb,&r2d_end_downc,TRUE);//dst

		   r2d_patch_branch(&r2d_skip_shift_up_upa,&r2d_end_upa,TRUE);//dst
		   r2d_patch_branch(&r2d_skip_shift_up_upb,&r2d_end_upb,TRUE);//src
		   r2d_patch_branch(&r2d_skip_shift_up_upc,&r2d_end_upc,TRUE);//dst

		   r2d_patch_inst(&r2d_always_read_down,&r2d_always_read_b_down1);
		   r2d_patch_inst(&r2d_always_read_down,&r2d_always_read_b_down2);
		   r2d_patch_inst(&r2d_always_read_up,&r2d_always_read_a_up1);
		   r2d_patch_inst(&r2d_always_read_up,&r2d_always_read_a_up2);
		}

        if ((srcdst==R2D_COLORLCD))
		{
           r2d_patch_code_block(&r2d_start_color_to_lcd,&r2d_sconvcolb_down1);
           r2d_patch_code_block(&r2d_start_color_to_lcd,&r2d_sconvcolb_down2);
           r2d_patch_code_block(&r2d_start_color_to_lcd,&r2d_sconvcola_up1);
           r2d_patch_code_block(&r2d_start_color_to_lcd,&r2d_sconvcola_up2);

		   // write_shift_pixel_down
		   r2d_patch_branch(&r2d_skip_load_src_a_up1,&r2d_no_null_src_counter_a,TRUE);//src
		   r2d_patch_branch(&r2d_skip_load_src_a_up2,&r2d_end_a_up2,TRUE);//src

		   // write_shift_pixel_up
		   r2d_patch_branch(&r2d_skip_load_src_b_down1,&r2d_no_null_src_counter_b,TRUE);//src
		   r2d_patch_branch(&r2d_skip_load_src_b_down2,&r2d_end_b_down2,TRUE);//src

		   

		   r2d_patch_branch(&r2d_skip_shift_down_downa,&r2d_end_downa,FALSE);// dst
		   r2d_patch_branch(&r2d_skip_shift_down_downb,&r2d_end_downb,TRUE);//src
		   r2d_patch_branch(&r2d_skip_shift_down_downb,&r2d_end_downc,FALSE);//dst

		   r2d_patch_branch(&r2d_skip_shift_up_upa,&r2d_end_upa,FALSE);//dst
		   r2d_patch_branch(&r2d_skip_shift_up_upb,&r2d_end_upb,TRUE);//src
		   r2d_patch_branch(&r2d_skip_shift_up_upc,&r2d_end_upc,FALSE);//dst

		   r2d_patch_inst(&r2d_always_read_down,&r2d_always_read_b_down1);
		   r2d_patch_inst(&r2d_always_read_down,&r2d_always_read_b_down2);
		   r2d_patch_inst(&r2d_always_read_up,&r2d_always_read_a_up1);
		   r2d_patch_inst(&r2d_always_read_up,&r2d_always_read_a_up2);
		}

		

        if ((srcdst==R2D_LCDCOLOR))
		{
           r2d_patch_code_block(&r2d_start_lcd_to_color,&r2d_sconvcolb_down1);
           r2d_patch_code_block(&r2d_start_lcd_to_color,&r2d_sconvcolb_down2);
           r2d_patch_code_block(&r2d_start_lcd_to_color,&r2d_sconvcola_up1);
           r2d_patch_code_block(&r2d_start_lcd_to_color,&r2d_sconvcola_up2);

		   // write_shift_pixel_down
		   r2d_patch_branch(&r2d_skip_load_src_a_up1,&r2d_no_null_src_counter_a,FALSE);//src
		   r2d_patch_branch(&r2d_skip_load_src_a_up2,&r2d_end_a_up2,FALSE);//src

		   // write_shift_pixel_up
		   r2d_patch_branch(&r2d_skip_load_src_b_down1,&r2d_no_null_src_counter_b,FALSE);//src
		   r2d_patch_branch(&r2d_skip_load_src_b_down2,&r2d_end_b_down2,FALSE);//src

		   

		   r2d_patch_branch(&r2d_skip_shift_down_downa,&r2d_end_downa,TRUE);// dst
		   r2d_patch_branch(&r2d_skip_shift_down_downb,&r2d_end_downb,FALSE);//src
		   r2d_patch_branch(&r2d_skip_shift_down_downb,&r2d_end_downc,TRUE);//dst

		   r2d_patch_branch(&r2d_skip_shift_up_upa,&r2d_end_upa,TRUE);//dst
		   r2d_patch_branch(&r2d_skip_shift_up_upb,&r2d_end_upb,FALSE);//src
		   r2d_patch_branch(&r2d_skip_shift_up_upc,&r2d_end_upc,TRUE);//dst

		   r2d_patch_inst(&r2d_never_write,&r2d_always_read_b_down1);
		   r2d_patch_inst(&r2d_never_write,&r2d_always_read_b_down2);
		   r2d_patch_inst(&r2d_never_write,&r2d_always_read_a_up1);
		   r2d_patch_inst(&r2d_never_write,&r2d_always_read_a_up2);
		}

	// Patch writing subroutines
    // Remove part of code not required when dst is R2D color
    // framebuffer (some branch are used to skip the parts)
    if ((srcdst==R2D_COLORCOLOR) || (srcdst==R2D_LCDCOLOR))
    {
       r2d_patch_inst(&r2d_always_write_down,&r2d_swb_down1);
       r2d_patch_inst(&r2d_always_write_down,&r2d_swb_down2);
       r2d_patch_inst(&r2d_always_write_up,&r2d_swa_up1);
       r2d_patch_inst(&r2d_always_write_up,&r2d_swa_up2);

       r2d_patch_branch(&r2d_skip_color_proc_a,&r2d_end_ynb_color_dst_a,TRUE);
       r2d_patch_branch(&r2d_skip_color_proc_b,&r2d_end_ynb_color_dst_b,TRUE);
       r2d_patch_branch(&r2d_skip_color_dst_a,&r2d_enda,TRUE);
       r2d_patch_branch(&r2d_skip_color_dst_b,&r2d_endb,TRUE);
    }
    else
    {
       r2d_patch_inst(&r2d_never_write,&r2d_swb_down1);
       r2d_patch_inst(&r2d_never_write,&r2d_swb_down2);
       r2d_patch_inst(&r2d_never_write,&r2d_swa_up1);
       r2d_patch_inst(&r2d_never_write,&r2d_swa_up2);

       r2d_patch_branch(&r2d_skip_color_proc_a,&r2d_end_ynb_color_dst_a,FALSE);
       r2d_patch_branch(&r2d_skip_color_proc_b,&r2d_end_ynb_color_dst_b,FALSE);
       r2d_patch_branch(&r2d_skip_color_dst_a,&r2d_enda,FALSE);
       r2d_patch_branch(&r2d_skip_color_dst_b,&r2d_endb,FALSE);
    }


    // Update or restore shifts related to src
    if ((srcdst==R2D_COLORCOLOR) || (srcdst==R2D_COLORLCD))
    {
        // Patch list for write_shift_pixel_down
        r2d_patch_shift(&r2d_sft_src_c_b_down1,0,1);
        r2d_patch_shift(&r2d_sft_src_c_b_down2,0,1);

        r2d_patch_shift(&r2d_sft_b_c_b_down1,0,0);
        r2d_patch_shift(&r2d_sft_b_c_b_down2,0,0);

        r2d_patch_shift(&r2d_sft_srcb__b_down1,0,0); // Read already done
        r2d_patch_shift(&r2d_sft_srcb__b_down2,0,0); // No need to make free area

        // Patch list for write_shift_pixel_up
        r2d_patch_shift(&r2d_sft_src_c_a_up1,0,0);
        r2d_patch_shift(&r2d_sft_src_c_a_up2,0,0);

        r2d_patch_shift(&r2d_sft_b_c_a_up1,0,0);
        r2d_patch_shift(&r2d_sft_b_c_a_up2,0,0);

        r2d_patch_shift(&r2d_sft_srcb__a_up1,0,1); // Read already done
        r2d_patch_shift(&r2d_sft_srcb__a_up2,0,1); // No need to make free area

        // Patch list for shift_pixel_down
       r2d_patch_shift(&r2d_sft_src_c_downb,0,1);
       r2d_patch_moveshift(&r2d_sft_srcb_n_downb,32,0);

        // Patch list for shift_pixel_up
       r2d_patch_shift(&r2d_sft_src_c_upb,0,0);
       r2d_patch_moveshift(&r2d_sft_srcb_n_upb,32,1);

    }
    else
    {
       // Patch list for write_shift_pixel_down
        r2d_patch_shift(&r2d_sft_src_c_b_down1,(1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH,1);
        r2d_patch_shift(&r2d_sft_src_c_b_down2,(1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH,1);

        r2d_patch_shift(&r2d_sft_b_c_b_down1,(1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH,0);
        r2d_patch_shift(&r2d_sft_b_c_b_down2,(1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH,0);

        r2d_patch_shift(&r2d_sft_srcb__b_down1,R2D_PIXEL_DEPTH,0);
        r2d_patch_shift(&r2d_sft_srcb__b_down2,R2D_PIXEL_DEPTH,0);

        // Patch list for write_shift_pixel_up
        r2d_patch_shift(&r2d_sft_src_c_a_up1,(1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH,0);
        r2d_patch_shift(&r2d_sft_src_c_a_up2,(1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH,0);

        r2d_patch_shift(&r2d_sft_b_c_a_up1,(1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH,0);
        r2d_patch_shift(&r2d_sft_b_c_a_up2,(1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH,0);

        r2d_patch_shift(&r2d_sft_srcb__a_up1,R2D_PIXEL_DEPTH,1);
        r2d_patch_shift(&r2d_sft_srcb__a_up2,R2D_PIXEL_DEPTH,1);

        // Patch list for shift_pixel_down
       r2d_patch_shift(&r2d_sft_src_c_downb,(1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH,1);
       r2d_patch_moveshift(&r2d_sft_srcb_n_downb,R2D_PIXEL_DEPTH,0);

        // Patch list for shift_pixel_up
       r2d_patch_shift(&r2d_sft_src_c_upb,(1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH,0);
       r2d_patch_moveshift(&r2d_sft_srcb_n_upb,R2D_PIXEL_DEPTH,1);
    }



    // Update (or restore) shifts related to dst
    if ((srcdst==R2D_COLORCOLOR) || (srcdst==R2D_LCDCOLOR))
    {
       // Patch list for write_shift_pixel_down
       r2d_patch_moveshift(&r2d_sft_dst_n_b_down1,32,0); // Shift when dst is R2D color
                                        // framebuffer.
                                        // Only instructions using dst
                                        // are impacted
       r2d_patch_moveshift(&r2d_sft_dst_n_b_down2,32,0);

       r2d_patch_shift(&r2d_sft_dstc__b_down1,0,0); // Already read
       r2d_patch_shift(&r2d_sft_dstc__b_down2,0,0); // No need to make free area

       r2d_patch_shift(&r2d_sft_dstb_c_b_down1,0,1);
       r2d_patch_shift(&r2d_sft_dstb_c_b_down2,0,1);
       r2d_patch_shift(&r2d_sft_d_c_b_down1,0,0);
       r2d_patch_shift(&r2d_sft_d_c_b_down2,0,0);
       r2d_patch_shift(&r2d_sft_dstc_c_b_down1,0,1);
       r2d_patch_shift(&r2d_sft_dstc_c_b_down2,0,1);



       // Patch list for write_shift_pixel_up
       r2d_patch_moveshift(&r2d_sft_dst_n_a_up1,32,1);
       r2d_patch_moveshift(&r2d_sft_dst_n_a_up2,32,1);

       r2d_patch_shift(&r2d_sft_dstc__a_up1,0,1); // Already read
       r2d_patch_shift(&r2d_sft_dstc__a_up2,0,1); // No need to make free area

       r2d_patch_shift(&r2d_sft_b_c_a_up1,0,0);
       r2d_patch_shift(&r2d_sft_b_c_a_up2,0,0);
       


       // Patch list for shift_pixel_down
       r2d_patch_moveshift(&r2d_sft_dst_n_downa,32,0);
       r2d_patch_moveshift(&r2d_sft_dst_n_downb,32,0);
       r2d_patch_moveshift(&r2d_sft_dst_n_downc,32,0);

       r2d_patch_shift(&r2d_sft_src_c_downa,0,1);
       r2d_patch_moveshift(&r2d_sft_srcb_n_downa,32,0);
       r2d_patch_shift(&r2d_sft_src_c_downc,0,1);
       r2d_patch_moveshift(&r2d_sft_srcb_n_downc,32,0);


       // Patch list for shift_pixel_up
       r2d_patch_moveshift(&r2d_sft_dst_n_upa,32,1);
       r2d_patch_moveshift(&r2d_sft_dst_n_upb,32,1);
       r2d_patch_moveshift(&r2d_sft_dst_n_upc,32,1);

       r2d_patch_shift(&r2d_sft_src_c_upa,0,0);
       r2d_patch_moveshift(&r2d_sft_srcb_n_upa,32,1);
       r2d_patch_shift(&r2d_sft_src_c_upc,0,0);
       r2d_patch_moveshift(&r2d_sft_srcb_n_upc,32,1);


    }
    else
    {
       
      
       // Patch list for write_shift_pixel_down
       r2d_patch_moveshift(&r2d_sft_dst_n_b_down1,R2D_PIXEL_DEPTH,0); // Shift when dst is R2D color
                                                     // framebuffer.
                                                     // Only instructions using dst
                                                     // are impacted
       r2d_patch_moveshift(&r2d_sft_dst_n_b_down2,R2D_PIXEL_DEPTH,0);

       r2d_patch_shift(&r2d_sft_dstc__b_down1,R2D_PIXEL_DEPTH,0);
       r2d_patch_shift(&r2d_sft_dstc__b_down2,R2D_PIXEL_DEPTH,0);

       r2d_patch_shift(&r2d_sft_dstb_c_b_down1,((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH),1);
       r2d_patch_shift(&r2d_sft_dstb_c_b_down2,((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH),1);
       r2d_patch_shift(&r2d_sft_d_c_b_down1,((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH),0);
       r2d_patch_shift(&r2d_sft_d_c_b_down2,((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH),0);
       r2d_patch_shift(&r2d_sft_dstc_c_b_down1,((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH),1);
       r2d_patch_shift(&r2d_sft_dstc_c_b_down2,((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH),1);

       // Patch list for write_shift_pixel_up
       r2d_patch_moveshift(&r2d_sft_dst_n_a_up1,R2D_PIXEL_DEPTH,1);
       r2d_patch_moveshift(&r2d_sft_dst_n_a_up2,R2D_PIXEL_DEPTH,1);

       r2d_patch_shift(&r2d_sft_dstc__a_up1,R2D_PIXEL_DEPTH,1);
       r2d_patch_shift(&r2d_sft_dstc__a_up2,R2D_PIXEL_DEPTH,1);

       r2d_patch_shift(&r2d_sft_b_c_a_up1,((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH),0);
       r2d_patch_shift(&r2d_sft_b_c_a_up2,((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH),0);
       

       // Patch list for shift_pixel_down
       r2d_patch_moveshift(&r2d_sft_dst_n_downa,R2D_PIXEL_DEPTH,0);
       r2d_patch_moveshift(&r2d_sft_dst_n_downb,R2D_PIXEL_DEPTH,0);
       r2d_patch_moveshift(&r2d_sft_dst_n_downc,R2D_PIXEL_DEPTH,0);

       r2d_patch_shift(&r2d_sft_src_c_downa,(1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH,1);
       r2d_patch_moveshift(&r2d_sft_srcb_n_downa,R2D_PIXEL_DEPTH,0);
       r2d_patch_shift(&r2d_sft_src_c_downc,(1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH,1);
       r2d_patch_moveshift(&r2d_sft_srcb_n_downc,R2D_PIXEL_DEPTH,0);


       // Patch list for shift_pixel_up
       r2d_patch_moveshift(&r2d_sft_dst_n_upa,R2D_PIXEL_DEPTH,1);
       r2d_patch_moveshift(&r2d_sft_dst_n_upb,R2D_PIXEL_DEPTH,1);
       r2d_patch_moveshift(&r2d_sft_dst_n_upc,R2D_PIXEL_DEPTH,1);

       r2d_patch_shift(&r2d_sft_src_c_upa,(1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH,0);
       r2d_patch_moveshift(&r2d_sft_srcb_n_upa,R2D_PIXEL_DEPTH,1);
       r2d_patch_shift(&r2d_sft_src_c_upc,(1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH,0);
       r2d_patch_moveshift(&r2d_sft_srcb_n_upc,R2D_PIXEL_DEPTH,1);

    }
	
   }
}


#endif