C++ ALIGN_DOWN函数代码示例

SYSCALL vm_addr VirtualAllocPhys (vm_addr addr, ssize_t len, bits32_t flags, vm_addr paddr)
{
    struct Segment *seg;
    struct Process *current;


    current = GetCurrentProcess();

    addr = ALIGN_DOWN (addr, PAGE_SIZE);
    paddr = ALIGN_DOWN (paddr, PAGE_SIZE);
    len = ALIGN_UP (len, PAGE_SIZE);
    flags = (flags & ~VM_SYSTEM_MASK) | MEM_PHYS;

    if (!(current->flags & PROCF_ALLOW_IO))
        return (vm_addr)NULL;

    if (PmapSupportsCachePolicy (flags) == -1)
        return (vm_addr)NULL;

    DisablePreemption();

    if (EnoughSegments() == FALSE)
        ReclaimSegments();
        
    if ((seg = SegmentAlloc (paddr, len, flags)) == NULL)
        return (vm_addr)NULL;
  
    return seg->base;
}

/** Unmap a piece of virtual address space.
 *
 * @param vaddr		Virtual address to be unmapped. May be unaligned, but
 *			it must a value previously returned by km_map().
 * @param size		Size of area starting at vaddr to be unmapped.
 */
void km_unmap(uintptr_t vaddr, size_t size)
{
	size_t offs; 

	offs = vaddr - ALIGN_DOWN(vaddr, PAGE_SIZE); 
	km_unmap_aligned(ALIGN_DOWN(vaddr, PAGE_SIZE),
	    ALIGN_UP(size + offs, PAGE_SIZE));
}

static int tegra_spi_dma_prepare(struct tegra_spi_channel *spi,
		unsigned int bytes, enum spi_direction dir)
{
	unsigned int todo, wcount;

	/*
	 * For DMA we need to think of things in terms of word count.
	 * AHB width is fixed at 32-bits. To avoid overrunning
	 * the in/out buffers we must align down. (Note: lowest 2-bits
	 * in WCOUNT register are ignored, and WCOUNT seems to count
	 * words starting at n-1)
	 *
	 * Example: If "bytes" is 7 and we are transferring 1-byte at a time,
	 * WCOUNT should be 4. The remaining 3 bytes must be transferred
	 * using PIO.
	 */
	todo = MIN(bytes, SPI_MAX_TRANSFER_BYTES_DMA - TEGRA_DMA_ALIGN_BYTES);
	todo = ALIGN_DOWN(todo, TEGRA_DMA_ALIGN_BYTES);
	wcount = ALIGN_DOWN(todo - TEGRA_DMA_ALIGN_BYTES, TEGRA_DMA_ALIGN_BYTES);

	flush_fifos(spi);

	if (dir == SPI_SEND) {
		spi->dma_out = dma_claim();
		if (!spi->dma_out)
			return -1;

		/* ensure bytes to send will be visible to DMA controller */
		dcache_clean_by_mva(spi->out_buf, bytes);

		write32(&spi->dma_out->regs->apb_ptr,
			(uintptr_t) & spi->regs->tx_fifo);
		write32(&spi->dma_out->regs->ahb_ptr, (uintptr_t)spi->out_buf);
		setbits_le32(&spi->dma_out->regs->csr, APB_CSR_DIR);
		setup_dma_params(spi, spi->dma_out);
		write32(&spi->dma_out->regs->wcount, wcount);
	} else {
		spi->dma_in = dma_claim();
		if (!spi->dma_in)
			return -1;

		/* avoid data collisions */
		dcache_clean_invalidate_by_mva(spi->in_buf, bytes);

		write32(&spi->dma_in->regs->apb_ptr,
			(uintptr_t)&spi->regs->rx_fifo);
		write32(&spi->dma_in->regs->ahb_ptr, (uintptr_t)spi->in_buf);
		clrbits_le32(&spi->dma_in->regs->csr, APB_CSR_DIR);
		setup_dma_params(spi, spi->dma_in);
		write32(&spi->dma_in->regs->wcount, wcount);
	}

	/* BLOCK_SIZE starts at n-1 */
	write32(&spi->regs->dma_blk, todo - 1);
	return todo;
}

/** Map a piece of physical address space into the virtual address space.
 *
 * @param paddr		Physical address to be mapped. May be unaligned.
 * @param size		Size of area starting at paddr to be mapped.
 * @param flags		Protection flags to be used for the mapping.
 *
 * @return New virtual address mapped to paddr.
 */
uintptr_t km_map(uintptr_t paddr, size_t size, unsigned int flags)
{
	uintptr_t page;
	size_t offs; 

	offs = paddr - ALIGN_DOWN(paddr, FRAME_SIZE); 
	page = km_map_aligned(ALIGN_DOWN(paddr, FRAME_SIZE),
	    ALIGN_UP(size + offs, FRAME_SIZE), flags);

	return page + offs;
}

void *write_thread(void *_ts) {
	struct ts *ts = _ts;
	struct packet *packet;

	mode_t umask_val = umask(0);
	dir_perm = (0777 & ~umask_val) | (S_IWUSR | S_IXUSR);

	set_thread_name("tsdump-write");
	while ((packet = queue_get(ts->packet_queue))) {
		if (!packet->data_len)
			continue;

		p_dbg1(" - Got packet %d, size: %u, file_time:%lu packet_time:%lu depth:%d\n",
			packet->num, packet->data_len, ALIGN_DOWN(packet->ts.tv_sec, ts->rotate_secs),
			packet->ts.tv_sec, ts->packet_queue->items);

		handle_files(ts, packet);

		if (ts->output_fd > -1) {
			p_dbg2(" - Writing into fd:%d size:%d file:%s\n", ts->output_fd, packet->data_len, ts->output_filename);
			ssize_t written = write(ts->output_fd, packet->data, packet->data_len);
			if (written != packet->data_len) {
				p_err("Can not write data (fd:%d written %zd of %d file:%s)",
					ts->output_fd, written, packet->data_len, ts->output_filename);
			}
		}
		free_packet(packet);
	}
	close_output_file(ts, NO_UNLINK);
	return NULL;
}

static void handle_files(struct ts *ts, struct packet *packet) {
	int file_time = ALIGN_DOWN(packet->ts.tv_sec, ts->rotate_secs);

	// Is this file already created?
	if (file_time <= ts->output_startts)
		return;

	close_output_file(ts, UNLINK_OLD);
	format_output_filename(ts, file_time);

	/*
	 * When tsdumper2 is started, try to continue writing into "current" file.
	 * This allows the program to be killed/restarted.
	 *
	 * If current file does not exist, create new file with the time of the start
	 * (not aligned to rotate_secs).
	 */
	int append = 0;
	if (ts->output_fd < 0) { // First file (or error).
		append = file_exists(ts->output_filename);
		if (!append) { // Create first file *NOT ALIGNED*
			format_output_filename(ts, packet->ts.tv_sec);
		}
	}

	ts->output_fd = append ? append_output_file(ts) : create_output_file(ts);
}

/*! 
 * \brief メモリプールの初期化
 * \param self メモリプールへのポインタ
 * \param buf バッファへのポインタ
 * \param size バッファのサイズ
 * \param alignment アラインメント
 *
 * \pre bufはalignmentでアラインメントされていなければならない。
 * \pre alignmentは2のべき乗でなければならない。
 */
void Pool_init(Pool *self, void *buf, size_t size, size_t alignment)
{
	BlockHeader *p;
	self->align_size = alignment;
	self->header_size = ALIGN_UP(sizeof(BlockHeader));
#ifdef POOL_DEBUG
	self->wall_size = (alignment > 32) ? alignment : 32;
	self->fail_count = -1;
#endif
	p = (BlockHeader *) buf;

	self->list_term.size = 0;
	self->list_term.occupied = 1;
	self->list_term.magic = ~MAGIC_NO & 0xFF;
	self->list_term.next = p;
#ifndef POOL_SLIST
	self->list_term.prev = p;
#endif

	p->size = ALIGN_DOWN(size);
	p->occupied = 0;
	p->magic = MAGIC_NO;
	p->next = &self->list_term;
#ifndef POOL_SLIST
	p->prev = &self->list_term;
#endif
	self->loop_p = &self->list_term;

	self->init_flag = self;
}

bool SoftwareWriteWatch::GetDirtyFromBlock(
    uint8_t *block,
    uint8_t *firstPageAddressInBlock,
    size_t startByteIndex,
    size_t endByteIndex,
    void **dirtyPages,
    size_t *dirtyPageIndexRef,
    size_t dirtyPageCount,
    bool clearDirty)
{
    assert(block != nullptr);
    assert(ALIGN_DOWN(block, sizeof(size_t)) == block);
    assert(firstPageAddressInBlock == reinterpret_cast<uint8_t *>(GetPageAddress(block - GetTable())));
    assert(startByteIndex < endByteIndex);
    assert(endByteIndex <= sizeof(size_t));
    assert(dirtyPages != nullptr);
    assert(dirtyPageIndexRef != nullptr);

    size_t &dirtyPageIndex = *dirtyPageIndexRef;
    assert(dirtyPageIndex < dirtyPageCount);

    size_t dirtyBytes = *reinterpret_cast<size_t *>(block);
    if (dirtyBytes == 0)
    {
        return true;
    }

    if (startByteIndex != 0)
    {
        size_t numLowBitsToClear = startByteIndex * 8;
        dirtyBytes >>= numLowBitsToClear;
        dirtyBytes <<= numLowBitsToClear;
    }

size_t strnlen(const char *s, size_t maxlen)
{
	const char *p;
	unsigned long r;
	long f, k;
	prefetch(s);

	/*
	 * Sometimes you need a new perspective, like the altivec
	 * way of handling things.
	 * Lower address bits? Totaly overestimated.
	 *
	 * We don't precheck for alignment.
	 * Instead we "align hard", do one load "under the address",
	 * mask the excess info out and afterwards we are fine to go.
	 *
	 * Even this beeing strNlen, this n can be seen as a "hint"
	 * We can overread and underread, but should cut the result.
	 */
	f = ALIGN_DOWN_DIFF(s, SOUL);
	k = SOUL - f - (long)maxlen;
	k = k > 0 ? k  : 0;

	p = (const char *)ALIGN_DOWN(s, SOUL);
	r = cmpbeqz(*(const unsigned long *)p);
	if(!HOST_IS_BIGENDIAN) {
		r <<= k     + SOULM1 * BITS_PER_CHAR;
		r >>= k + f + SOULM1 * BITS_PER_CHAR;
	} else {

static void imdr_init(struct imdr *ir, void *upper_limit)
{
	uintptr_t limit = (uintptr_t)upper_limit;
	/* Upper limit is aligned down to 4KiB */
	ir->limit = ALIGN_DOWN(limit, LIMIT_ALIGN);
	ir->r = NULL;
}

void *my_memchr(const void *s, int c, size_t n)
{
	const unsigned char *p;
	size_t r, mask;
	ssize_t f, k;

	prefetch(s);
	/*
	 * Sometimes you need a new perspective, like the altivec
	 * way of handling things.
	 * Lower address bits? Totaly overestimated.
	 *
	 * We don't precheck for alignment.
	 * Instead we "align hard", do one load "under the address",
	 * mask the excess info out and afterwards we are fine to go.
	 *
	 * Even this beeing a mem func with a len, this len can be seen
	 * as a "hint". We can overread and underread, but should cut
	 * the result and not hit a page fault.
	 */
	f = ALIGN_DOWN_DIFF((const unsigned char *)s, SOST);
	k = SOST - f - (ssize_t)n;
	k = k > 0 ? k  : 0;
	mask = (c & 0xFF) * MK_C(0x01010101);

	p  = (const unsigned char *)ALIGN_DOWN((const unsigned char *)s, SOST);
	r  = *(const size_t *)p;
	if(!HOST_IS_BIGENDIAN)
		r >>= f * BITS_PER_CHAR;
	r  = has_eq_byte(r, mask);
	if(!HOST_IS_BIGENDIAN) {
		r <<=  k      * BITS_PER_CHAR;
		r >>= (k + f) * BITS_PER_CHAR;
	} else {

static int exynos_rng_set_seed(struct exynos_rng_dev *rng,
			       const u8 *seed, unsigned int slen)
{
	u32 val;
	int i;

	/* Round seed length because loop iterates over full register size */
	slen = ALIGN_DOWN(slen, 4);

	if (slen < EXYNOS_RNG_SEED_SIZE)
		return -EINVAL;

	for (i = 0; i < slen ; i += 4) {
		unsigned int seed_reg = (i / 4) % EXYNOS_RNG_SEED_REGS;

		val = seed[i] << 24;
		val |= seed[i + 1] << 16;
		val |= seed[i + 2] << 8;
		val |= seed[i + 3] << 0;

		exynos_rng_writel(rng, val, EXYNOS_RNG_SEED(seed_reg));
	}

	val = exynos_rng_readl(rng, EXYNOS_RNG_STATUS);
	if (!(val & EXYNOS_RNG_STATUS_SEED_SETTING_DONE)) {
		dev_warn(rng->dev, "Seed setting not finished\n");
		return -EIO;
	}

	rng->last_seeding = jiffies;
	rng->bytes_seeding = 0;

	return 0;
}

ULONG
NTAPI
ObpHash(IN PVOID Buffer,
        IN ULONG Length)
{
    PULONG p, pp;
    PUCHAR pb, ppb;
    ULONG Hash = 0;

    /* Setup aligned and byte buffers */
    p = Buffer;
    pb = (PUCHAR)p;
    ppb = (PUCHAR)((ULONG_PTR)Buffer + Length);
    pp = (PULONG)ALIGN_DOWN(pb + Length, ULONG);

    /* Loop aligned data */
    while (p < pp)
    {
        /* XOR-rotate */
        Hash ^= *p++;
        Hash = _rotl(Hash, 3);
    }

    /* Loop non-aligned data */
    pb = (PUCHAR)p;
    while (pb < ppb)
    {
        /* XOR-rotate */
        Hash ^= *pb++;
        Hash = _rotl(Hash, 3);
    }

    /* Return the hash */
    return Hash;
}

static int cbfs_master_header_props(struct cbfs_props *props)
{
	struct cbfs_header header;
	int32_t offset;
	const struct region_device *bdev;

	bdev = boot_device_ro();

	rdev_readat(bdev, &offset, CONFIG_ROM_SIZE - sizeof(offset),
			sizeof(offset));

	/* The offset is relative to the end of the media. */
	offset += CONFIG_ROM_SIZE;

	rdev_readat(bdev, &header , offset, sizeof(header));

	header.magic = ntohl(header.magic);
	header.romsize = ntohl(header.romsize);
	header.bootblocksize = ntohl(header.bootblocksize);
	header.offset = ntohl(header.offset);

	if (header.magic != CBFS_HEADER_MAGIC)
		return -1;

	props->offset = header.offset;
	if (CONFIG_ROM_SIZE != header.romsize)
		props->size = CONFIG_ROM_SIZE;
	else
		props->size = header.romsize;
	props->size -= props->offset;
	props->size -= header.bootblocksize;
	props->size = ALIGN_DOWN(props->size, 64);

	return 0;
}

static void *expected_addr(void *in_addr, void *out_addr)
{
	if (!in_addr)
		return out_addr;

	return ALIGN_DOWN(in_addr);
}