void SubVectorsSIMD(float* c, const float* a, const float* b, std::size_t n) {
     std::size_t i = 0;
     for (; i < ROUND_DOWN(n, 4); i += 4) {
         __m128 ma = _mm_loadu_ps(a + i);
         __m128 mb = _mm_loadu_ps(b + i);
         __m128 mc = _mm_sub_ps(ma, mb);
         _mm_storeu_ps(c + i, mc);
     }
     for (; i < n; i++) {
         c[i] = a[i] - b[i];
     }
 }

static int set_qp_roi(int stream_id)
{
	iav_qp_roi_matrix_ex_t qp_matrix;
	int i, j;
	u32 *addr = (u32 *)(qp_matrix_addr + stream_qp_matrix_size * stream_id);
	u32 buf_width, buf_pitch, buf_height, start_x, start_y, end_x, end_y;
	VERIFY_STREAMID(stream_id);
	if (get_qp_roi(stream_id, &qp_matrix) < 0)
		return -1;
	qp_matrix.enable = 1;

	// QP matrix is MB level. One MB is 16x16 pixels.
	buf_width = ROUND_UP(stream_roi[stream_id].encode_width, 16) / 16;
	buf_pitch = ROUND_UP(buf_width, 8);
	buf_height = ROUND_UP(stream_roi[stream_id].encode_height, 16) / 16;

	start_x = ROUND_DOWN(qp_roi[stream_id].start_x, 16) / 16;
	start_y = ROUND_DOWN(qp_roi[stream_id].start_y, 16) / 16;
	end_x = ROUND_UP(qp_roi[stream_id].width, 16) / 16 + start_x;
	end_y = ROUND_UP(qp_roi[stream_id].height, 16) / 16 + start_y;

	for (i = start_y; i < end_y && i < buf_height; i++) {
		for (j = start_x; j < end_x && j < buf_width; j++)
			addr[i * buf_pitch + j] = qp_roi[stream_id].quality_level;
	}

	if (ioctl(fd_iav, IAV_IOC_SET_QP_ROI_MATRIX_EX, &qp_matrix) < 0) {
		perror("IAV_IOC_SET_QP_ROI_MATRIX_EX");
		return -1;
	}
	return 0;
}

QString SpimView::formatUserStack()
{
    if (st_showUserStackSegment)
    {
        return formatSegLabel("<br>User Stack", ROUND_DOWN(R[29], BYTES_PER_WORD), STACK_TOP)
            % formatMemoryContents(ROUND_DOWN(R[29], BYTES_PER_WORD), STACK_TOP);
    }
    else
    {
        return QString("");
    }
}

int
ImagingDrawWideLine(Imaging im, int x0, int y0, int x1, int y1,
                    const void* ink_, int width, int op)
{
    DRAW* draw;
    INT32 ink;
    int dx, dy;
    double big_hypotenuse, small_hypotenuse, ratio_max, ratio_min;
    int dxmin, dxmax, dymin, dymax;
    Edge e[4];

    DRAWINIT();

    if (width <= 1) {
        draw->line(im, x0, y0, x1, y1, ink);
        return 0;
    }

    dx = x1-x0;
    dy = y1-y0;
    if (dx == 0 && dy == 0) {
        draw->point(im, x0, y0, ink);
        return 0;
    }

    big_hypotenuse = sqrt((double) (dx*dx + dy*dy));
    small_hypotenuse = (width - 1) / 2.0;
    ratio_max = ROUND_UP(small_hypotenuse) / big_hypotenuse;
    ratio_min = ROUND_DOWN(small_hypotenuse) / big_hypotenuse;

    dxmin = ROUND_DOWN(ratio_min * dy);
    dxmax = ROUND_DOWN(ratio_max * dy);
    dymin = ROUND_DOWN(ratio_min * dx);
    dymax = ROUND_DOWN(ratio_max * dx);
    {
        int vertices[4][2] = {
            {x0 - dxmin, y0 + dymax},
            {x1 - dxmin, y1 + dymax},
            {x1 + dxmax, y1 - dymin},
            {x0 + dxmax, y0 - dymin}
        };

        add_edge(e+0, vertices[0][0], vertices[0][1], vertices[1][0], vertices[1][1]);
        add_edge(e+1, vertices[1][0], vertices[1][1], vertices[2][0], vertices[2][1]);
        add_edge(e+2, vertices[2][0], vertices[2][1], vertices[3][0], vertices[3][1]);
        add_edge(e+3, vertices[3][0], vertices[3][1], vertices[0][0], vertices[0][1]);

        draw->polygon(im, 4, e, ink, 0);
    }
    return 0;
}

static void
cmd_ctrl_u(key_info_t key_info, keys_info_t *keys_info)
{
	if(!at_first_line(view))
	{
		int new_pos;
		size_t offset = view->window_cells/2;
		offset = ROUND_DOWN(offset, view->column_count);
		new_pos = get_corrected_list_pos_up(view, offset);
		new_pos = MIN(new_pos, view->list_pos - (int)offset);
		new_pos = MAX(new_pos, 0);
		new_pos = ROUND_DOWN(new_pos, view->column_count);
		view->top_line += new_pos - view->list_pos;
		goto_pos(new_pos);
	}
}

void pdsp::SquarePeakDetector::formulaAudioRate(float* &output, const float* &input, const int &bufferSize) noexcept {
    
    int n = 0;
    int maxSimd = ROUND_DOWN(bufferSize, 16); 

#ifdef OFX_SIMD_USE_SIMD
    for ( ; n<maxSimd; n+=16){
        ofx::f128 x0  = ofx::m_load(input + n);
        ofx::f128 x4  = ofx::m_load(input + n + 4);
        ofx::f128 x8  = ofx::m_load(input + n + 8);
        ofx::f128 x12 = ofx::m_load(input + n + 12);
        
        x0  = ofx::m_mul(x0,  x0 );
        x4  = ofx::m_mul(x4,  x4 );
        x8  = ofx::m_mul(x8,  x8 );
        x12 = ofx::m_mul(x12, x12);
        
        ofx::m_store(output + n,      x0 ); 
        ofx::m_store(output + n + 4,  x4 ); 
        ofx::m_store(output + n + 8,  x8 ); 
        ofx::m_store(output + n + 12, x12); 
    }
#endif
    
    for( ; n<bufferSize; ++n){
        output[n] = input[n] * input[n];
    }
}

void CBoot::Load_FST(bool _bIsWii)
{
  if (!DVDInterface::VolumeIsValid())
    return;

  const DiscIO::IVolume& volume = DVDInterface::GetVolume();

  // copy first 32 bytes of disc to start of Mem 1
  DVDRead(/*offset*/ 0, /*address*/ 0, /*length*/ 0x20, false);

  // copy of game id
  Memory::Write_U32(Memory::Read_U32(0x0000), 0x3180);

  u32 shift = 0;
  if (_bIsWii)
    shift = 2;

  u32 fst_offset = 0;
  u32 fst_size = 0;
  u32 max_fst_size = 0;

  volume.ReadSwapped(0x0424, &fst_offset, _bIsWii);
  volume.ReadSwapped(0x0428, &fst_size, _bIsWii);
  volume.ReadSwapped(0x042c, &max_fst_size, _bIsWii);

  u32 arena_high = ROUND_DOWN(0x817FFFFF - (max_fst_size << shift), 0x20);
  Memory::Write_U32(arena_high, 0x00000034);

  // load FST
  DVDRead(fst_offset << shift, arena_high, fst_size << shift, _bIsWii);
  Memory::Write_U32(arena_high, 0x00000038);
  Memory::Write_U32(max_fst_size << shift, 0x0000003c);
}

/*
 * Note that we cannot use ioremap for SRAM, as clock init needs SRAM early.
 */
static void __init omap_map_sram(void)
{
	int cached = 1;

	if (omap_sram_size == 0)
		return;

	if (cpu_is_omap34xx()) {
		/*
		 * SRAM must be marked as non-cached on OMAP3 since the
		 * CORE DPLL M2 divider change code (in SRAM) runs with the
		 * SDRAM controller disabled, and if it is marked cached,
		 * the ARM may attempt to write cache lines back to SDRAM
		 * which will cause the system to hang.
		 */
		cached = 0;
	}

	omap_sram_start = ROUND_DOWN(omap_sram_start, PAGE_SIZE);
	omap_sram_base = __arm_ioremap_exec(omap_sram_start, omap_sram_size,
						cached);
	if (!omap_sram_base) {
		pr_err("SRAM: Could not map\n");
		return;
	}

	omap_sram_ceil = omap_sram_base + omap_sram_size;

	/*
	 * Looks like we need to preserve some bootloader code at the
	 * beginning of SRAM for jumping to flash for reboot to work...
	 */
	memset((void *)omap_sram_base + SRAM_BOOTLOADER_SZ, 0,
	       omap_sram_size - SRAM_BOOTLOADER_SZ);
}

void pdsp::PositiveValue::formulaAudioRate(float* &output, const float* &input, const int &bufferSize) noexcept {
        
        int n = 0;
        int maxSimd = ROUND_DOWN(bufferSize, 16); 

#ifdef OFX_SIMD_USE_SIMD
        for ( ; n<maxSimd; n+=16){
                ofx::f128 x0  = ofx::m_load(input + n);
                ofx::f128 x4  = ofx::m_load(input + n + 4);
                ofx::f128 x8  = ofx::m_load(input + n + 8);
                ofx::f128 x12 = ofx::m_load(input + n + 12);
                
                x0  = ofx::m_max1(x0,  0.0f);
                x4  = ofx::m_max1(x4,  0.0f);
                x8  = ofx::m_max1(x8,  0.0f);
                x12 = ofx::m_max1(x12, 0.0f);
                
                ofx::m_store(output + n,      x0 ); 
                ofx::m_store(output + n + 4,  x4 ); 
                ofx::m_store(output + n + 8,  x8 ); 
                ofx::m_store(output + n + 12, x12); 
        }
#endif        
        for( ; n<bufferSize; ++n){
                if(input[n] > 0.0f){
                        output[n] = input[n];
                }else{
                        output[n] = 0.0f;
                } 
        }
}

static position_t isInBuffer(Buffer_t *This, mt_off_t start, size_t *len)
{
	if(start >= This->current &&
	   start < This->current + This->cur_size) {
		maximize(*len, This->cur_size - OFFSET);
		return INSIDE;
	} else if(start == This->current + This->cur_size &&
		  This->cur_size < This->size &&
		  *len >= This->sectorSize) {
		/* append to the buffer for this, three conditions have to
		 * be met:
		 *  1. The start falls exactly at the end of the currently
		 *     loaded data
		 *  2. There is still space
		 *  3. We append at least one sector
		 */
		maximize(*len, This->size - This->cur_size);
		*len = ROUND_DOWN(*len, This->sectorSize);
		return APPEND;
	} else {
		if(invalidate_buffer(This, start) < 0)
			return ERROR;
		maximize(*len, This->cylinderSize - OFFSET);
		maximize(*len, This->cylinderSize - This->current % This->cylinderSize);
		return OUTSIDE;
	}
}

unsigned char *arch_stack_init(void *tentry, void *a, void *b,
                               char *stack_low, char *stack_high, void *texit)
{
    int *p;
    
    p = (int)ROUND_DOWN(stack_high, 8);

    /*
     *  r0-r12, lr, pc
     */
    *p-- = (int)tentry;  /* pc */
    *p-- = (int)texit;  /* lr */
    *p-- = 0xcccccccc;                  /* r12*/
    *p-- = 0xbbbbbbbb;                  /* r11 */
    *p-- = 0xaaaaaaaa;                  /* r10 */
    *p-- = 0x99999999;                  /* r9 */
    *p-- = 0x88888888;                  /* r8 */
    *p-- = 0x77777777;                  /* r7 */
    *p-- = 0x66666666;                  /* r6 */
    *p-- = 0x55555555;                  /* r5 */
    *p-- = 0x44444444;                  /* r4*/
    *p-- = 0x33333333;                  /* r3*/
    *p-- = 0x22222222;                  /* r2*/
    *p-- = b;                  /* r1: arg1 of function task_entry_exit */
    *p-- = a;                  /* r0: arg0 of function task_entry_exit */
    *p-- = STATUS_REG_INIT_VALUE; /* cpsr */
    *p = STATUS_REG_INIT_VALUE; /* spsr */
    
    /*
     *  full stack pointer
     */
    return p;
}

static
PCACHE_SECTION_PAGE_TABLE
NTAPI
MiSectionPageTableGetOrAllocate(PRTL_GENERIC_TABLE Table,
                                PLARGE_INTEGER FileOffset)
{
    LARGE_INTEGER SearchFileOffset;
    CACHE_SECTION_PAGE_TABLE SectionZeroPageTable;
    PCACHE_SECTION_PAGE_TABLE PageTableSlice = MiSectionPageTableGet(Table,
                                                                     FileOffset);
    /* Please zero memory when taking away zero initialization. */
    RtlZeroMemory(&SectionZeroPageTable, sizeof(CACHE_SECTION_PAGE_TABLE));
    if (!PageTableSlice)
    {
        SearchFileOffset.QuadPart = ROUND_DOWN(FileOffset->QuadPart,
                                               ENTRIES_PER_ELEMENT * PAGE_SIZE);
        SectionZeroPageTable.FileOffset = SearchFileOffset;
        SectionZeroPageTable.Refcount = 1;
        PageTableSlice = RtlInsertElementGenericTable(Table,
                                                      &SectionZeroPageTable,
                                                      sizeof(SectionZeroPageTable),
                                                      NULL);
        if (!PageTableSlice) return NULL;
        DPRINT("Allocate page table %p (%I64x)\n",
               PageTableSlice,
               PageTableSlice->FileOffset.QuadPart);
    }
    return PageTableSlice;
}

/*
 * FUNCTION: Retrieve the next FAT16 cluster from the FAT table
 */
NTSTATUS
FAT16GetNextCluster(
    PDEVICE_EXTENSION DeviceExt,
    ULONG CurrentCluster,
    PULONG NextCluster)
{
    PVOID BaseAddress;
    ULONG FATOffset;
    ULONG ChunkSize;
    PVOID Context;
    LARGE_INTEGER Offset;

    ChunkSize = CACHEPAGESIZE(DeviceExt);
    FATOffset = CurrentCluster * 2;
    Offset.QuadPart = ROUND_DOWN(FATOffset, ChunkSize);
    if (!CcMapData(DeviceExt->FATFileObject, &Offset, ChunkSize, MAP_WAIT, &Context, &BaseAddress))
    {
         return STATUS_UNSUCCESSFUL;
    }

    CurrentCluster = *((PUSHORT)((char*)BaseAddress + (FATOffset % ChunkSize)));
    if (CurrentCluster >= 0xfff8 && CurrentCluster <= 0xffff)
        CurrentCluster = 0xffffffff;
    CcUnpinData(Context);
    *NextCluster = CurrentCluster;
    return STATUS_SUCCESS;
}

ULONG_PTR
NTAPI
_MmGetPageEntrySectionSegment(PMM_SECTION_SEGMENT Segment,
                              PLARGE_INTEGER Offset,
                              const char *file,
                              int line)
{
    LARGE_INTEGER FileOffset;
    ULONG_PTR PageIndex, Result;
    PCACHE_SECTION_PAGE_TABLE PageTable;

    ASSERT(Segment->Locked);
    FileOffset.QuadPart = ROUND_DOWN(Offset->QuadPart,
                                     ENTRIES_PER_ELEMENT * PAGE_SIZE);
    PageTable = MiSectionPageTableGet(&Segment->PageTable, &FileOffset);
    if (!PageTable) return 0;
    PageIndex = (ULONG_PTR)((Offset->QuadPart - PageTable->FileOffset.QuadPart) / PAGE_SIZE);
    Result = PageTable->PageEntries[PageIndex];
#if 0
    DPRINTC
        ("MiGetPageEntrySectionSegment(%p,%08x%08x) => %x %s:%d\n",
         Segment,
         FileOffset.u.HighPart,
         FileOffset.u.LowPart + PageIndex * PAGE_SIZE,
         Result,
         file, line);
#endif
    return Result;
}

void CBoot::Load_FST(bool _bIsWii)
{
	if (!VolumeHandler::IsValid())
		return;

	// copy first 20 bytes of disc to start of Mem 1
	VolumeHandler::ReadToPtr(Memory::GetPointer(0x80000000), 0, 0x20);

	// copy of game id
	Memory::Write_U32(Memory::Read_U32(0x80000000), 0x80003180);

	u32 shift = 0;
	if (_bIsWii)
		shift = 2;

	u32 fstOffset  = VolumeHandler::Read32(0x0424) << shift;
	u32 fstSize    = VolumeHandler::Read32(0x0428) << shift;
	u32 maxFstSize = VolumeHandler::Read32(0x042c) << shift;

	u32 arenaHigh = ROUND_DOWN(0x817FFFFF - maxFstSize, 0x20);
	Memory::Write_U32(arenaHigh, 0x00000034);

	// load FST
	VolumeHandler::ReadToPtr(Memory::GetPointer(arenaHigh), fstOffset, fstSize);
	Memory::Write_U32(arenaHigh, 0x00000038);
	Memory::Write_U32(maxFstSize, 0x0000003c);
}

void *phys_map(phys_addr_t addr, size_t size, int mmflag)
{
	phys_addr_t base;
	phys_addr_t end;
	void *ret = NULL;

	if (!size) {
		/* What do you want to do??? */
		ASSERT(0);
		goto out;
	}

	/* Use the physical map area if the range lies within it. For more
	 * information please refer to the memory layout of our system.
	 */
	if (PMAP_CONTAINS(addr, size)) {
		ret = (void *)addr;
		goto out;
	}

	base = ROUND_DOWN(addr, PAGE_SIZE);
	end = ROUND_UP(addr + size, PAGE_SIZE);
	ASSERT(end > base);
	
	/* Map pages from kernel memory */
	ret = kmem_map(base, end - base, mmflag);
	if (ret) {
		ret = (char *)ret + (addr - base);	// Don't miss the offset
	}
	
	DEBUG(DL_DBG, ("addr(%x), size(%x), ret(%p).\n", addr, size, ret));

 out:
	return ret;
}

int main()
{
	int x = FLASH_BLOCK * 5+1;
	printf("round_up = %x\n",ROUND_UP(x,FLASH_BLOCK));
	printf("round_down = %x\n",ROUND_DOWN(x,FLASH_BLOCK));
	return 0;
}

static void
cmd_ctrl_d(key_info_t key_info, keys_info_t *keys_info)
{
	if(!at_last_line(view))
	{
		size_t new_pos;
		size_t offset = view->window_cells/2;
		offset = ROUND_DOWN(offset, view->column_count);
		new_pos = get_corrected_list_pos_down(view, offset);
		new_pos = MAX(new_pos, view->list_pos + offset);
		new_pos = MIN(new_pos, (size_t)view->list_rows);
		new_pos = ROUND_DOWN(new_pos, view->column_count);
		view->top_line += new_pos - view->list_pos;
		goto_pos(new_pos);
	}
}

NTSTATUS
FAT32FindAndMarkAvailableCluster (PDEVICE_EXTENSION DeviceExt, PULONG Cluster)
/*
 * FUNCTION: Finds the first available cluster in a FAT32 table
 */
{
  ULONG FatLength;
  ULONG StartCluster;
  ULONG i, j;
  PVOID BaseAddress;
  ULONG ChunkSize;
  PVOID Context;
  LARGE_INTEGER Offset;
  PULONG Block;
  PULONG BlockEnd;

  ChunkSize = CACHEPAGESIZE(DeviceExt);
  FatLength = (DeviceExt->FatInfo.NumberOfClusters + 2);
  *Cluster = 0;
  StartCluster = DeviceExt->LastAvailableCluster;

  for (j = 0; j < 2; j++)
  {
    for (i = StartCluster; i < FatLength;)
    {
      Offset.QuadPart = ROUND_DOWN(i * 4, ChunkSize);
      if(!CcPinRead(DeviceExt->FATFileObject, &Offset, ChunkSize, 1, &Context, &BaseAddress))
      {
        DPRINT1("CcMapData(Offset %x, Length %d) failed\n", (ULONG)Offset.QuadPart, ChunkSize);
        return STATUS_UNSUCCESSFUL;
      }
      Block = (PULONG)((ULONG_PTR)BaseAddress + (i * 4) % ChunkSize);
      BlockEnd = (PULONG)((ULONG_PTR)BaseAddress + ChunkSize);

      /* Now process the whole block */
      while (Block < BlockEnd && i < FatLength)
      {
        if ((*Block & 0x0fffffff) == 0)
        {
          DPRINT("Found available cluster 0x%x\n", i);
          DeviceExt->LastAvailableCluster = *Cluster = i;
          *Block = 0x0fffffff;
          CcSetDirtyPinnedData(Context, NULL);
          CcUnpinData(Context);
          if (DeviceExt->AvailableClustersValid)
            InterlockedDecrement((PLONG)&DeviceExt->AvailableClusters);
          return(STATUS_SUCCESS);
        }

        Block++;
        i++;
      }

      CcUnpinData(Context);
    }
    FatLength = StartCluster;
    StartCluster = 2;
  }
  return (STATUS_DISK_FULL);
}

NTSTATUS
FAT32GetNextCluster(PDEVICE_EXTENSION DeviceExt,
		    ULONG CurrentCluster,
		    PULONG NextCluster)
/*
 * FUNCTION: Retrieve the next FAT32 cluster from the FAT table via a physical
 *           disk read
 */
{
  PVOID BaseAddress;
  ULONG FATOffset;
  ULONG ChunkSize;
  PVOID Context;
  LARGE_INTEGER Offset;

  ChunkSize = CACHEPAGESIZE(DeviceExt);
  FATOffset = CurrentCluster * sizeof(ULONG);
  Offset.QuadPart = ROUND_DOWN(FATOffset, ChunkSize);
  if(!CcMapData(DeviceExt->FATFileObject, &Offset, ChunkSize, 1, &Context, &BaseAddress))
  {
    return STATUS_UNSUCCESSFUL;
  }
  CurrentCluster = (*(PULONG)((char*)BaseAddress + (FATOffset % ChunkSize))) & 0x0fffffff;
  if (CurrentCluster >= 0xffffff8 && CurrentCluster <= 0xfffffff)
    CurrentCluster = 0xffffffff;
  CcUnpinData(Context);
  *NextCluster = CurrentCluster;
  return (STATUS_SUCCESS);
}