Differential D37878 Diff 165091 sys/contrib/vchiq/interface/vchiq_arm/vchiq_2835_arm.c

Changeset View

Standalone View

sys/contrib/vchiq/interface/vchiq_arm/vchiq_2835_arm.c

Show First 20 Lines • Show All 59 Lines • ▼ Show 20 Lines

#include "vchiq_arm.h"		#include "vchiq_arm.h"
#include "vchiq_2835.h"		#include "vchiq_2835.h"
#include "vchiq_connected.h"		#include "vchiq_connected.h"
#include "vchiq_killable.h"		#include "vchiq_killable.h"

#define MAX_FRAGMENTS (VCHIQ_NUM_CURRENT_BULKS * 2)		#define MAX_FRAGMENTS (VCHIQ_NUM_CURRENT_BULKS * 2)

		/*
		* XXXMDC
		* Do this less ad-hoc-y -- e.g.
		* https://github.com/raspberrypi/linux/commit/c683db8860a80562a2bb5b451d77b3e471d24f36
		*/
		#if defined(__aarch64__)
		int g_cache_line_size = 64;
		#else
int g_cache_line_size = 32;		int g_cache_line_size = 32;
		#endif
static int g_fragment_size;		static int g_fragment_size;

		unsigned int g_long_bulk_space = 0;
		#define VM_PAGE_TO_VC_BULK_PAGE(x) (\
		g_long_bulk_space ? VM_PAGE_TO_PHYS(x)\
		: PHYS_TO_VCBUS(VM_PAGE_TO_PHYS(x))\
		)

typedef struct vchiq_2835_state_struct {		typedef struct vchiq_2835_state_struct {
int inited;		int inited;
VCHIQ_ARM_STATE_T arm_state;		VCHIQ_ARM_STATE_T arm_state;
} VCHIQ_2835_ARM_STATE_T;		} VCHIQ_2835_ARM_STATE_T;

static char *g_slot_mem;		static char *g_slot_mem;
static int g_slot_mem_size;		static int g_slot_mem_size;
vm_paddr_t g_slot_phys;		vm_paddr_t g_slot_phys;
Show All 29 Lines	vchiq_dmamap_cb(void arg, bus_dma_segment_t segs, int nseg, int err)

if (err)		if (err)
return;		return;

addr = (bus_addr_t*)arg;		addr = (bus_addr_t*)arg;
*addr = PHYS_TO_VCBUS(segs[0].ds_addr);		*addr = PHYS_TO_VCBUS(segs[0].ds_addr);
}		}

		#if defined(__aarch64__) /* See comment in free_pagelist */
static int		static int
		invalidate_cachelines_in_range_of_ppage(
		vm_page_t p,
		size_t offset,
		size_t count
		)
		{
		if(offset + count > PAGE_SIZE){ return EINVAL; }
		uint8_t dst = (uint8_t)pmap_quick_enter_page(p);
		if (!dst){
		return ENOMEM;
		}
		cpu_dcache_inv_range((void *)((vm_offset_t)dst + offset), count);
		pmap_quick_remove_page((vm_offset_t)dst);
		return 0;
		}

		/* XXXMDC bulk instead of loading and invalidating single pages? */
		static void
		invalidate_cachelines_in_range_of_ppage_seq(
		vm_page_t *p,
		size_t start,
		size_t count
		)
		{
		if(start >= PAGE_SIZE) goto invalid_input;

		#define _NEXT_AT(x,_m) (((x)+((_m)-1)) & ~((_m)-1)) /* for power of two m */
		size_t offset = _NEXT_AT(start,g_cache_line_size);
		#undef _NEXT_AT
		count = (offset < start + count) ? count - (offset - start) : 0;
		offset = offset & (PAGE_SIZE - 1);
		for(
		size_t done = 0;
		count > done;
		p++, done += PAGE_SIZE - offset, offset = 0
		){
		size_t in_page = PAGE_SIZE - offset;
		size_t todo = (count-done > in_page) ? in_page : count-done;
		int e = invalidate_cachelines_in_range_of_ppage(*p, offset, todo);
		if(e != 0)
		goto problem_in_loop;
		}
		return;

		problem_in_loop:
		invalid_input:
		WARN_ON(1);
		return;
		}
		#endif

		static int
copyout_page(vm_page_t p, size_t offset, void *kaddr, size_t size)		copyout_page(vm_page_t p, size_t offset, void *kaddr, size_t size)
{		{
uint8_t *dst;		uint8_t *dst;

dst = (uint8_t*)pmap_quick_enter_page(p);		dst = (uint8_t*)pmap_quick_enter_page(p);
if (!dst)		if (!dst)
return ENOMEM;		return ENOMEM;

▲ Show 20 Lines • Show All 41 Lines • ▼ Show 20 Lines	err = bus_dmamap_load(bcm_slots_dma_tag, bcm_slots_dma_map, g_slot_mem,
&g_slot_phys, 0);		&g_slot_phys, 0);

if (err) {		if (err) {
vchiq_log_error(vchiq_core_log_level, "cannot load DMA map");		vchiq_log_error(vchiq_core_log_level, "cannot load DMA map");
err = -ENOMEM;		err = -ENOMEM;
goto failed_load;		goto failed_load;
}		}

WARN_ON(((int)g_slot_mem & (PAGE_SIZE - 1)) != 0);		WARN_ON(((size_t)g_slot_mem & (PAGE_SIZE - 1)) != 0);

vchiq_slot_zero = vchiq_init_slots(g_slot_mem, g_slot_mem_size);		vchiq_slot_zero = vchiq_init_slots(g_slot_mem, g_slot_mem_size);
if (!vchiq_slot_zero) {		if (!vchiq_slot_zero) {
err = -EINVAL;		err = -EINVAL;
goto failed_init_slots;		goto failed_init_slots;
}		}

vchiq_slot_zero->platform_data[VCHIQ_PLATFORM_FRAGMENTS_OFFSET_IDX] =		vchiq_slot_zero->platform_data[VCHIQ_PLATFORM_FRAGMENTS_OFFSET_IDX] =
Show All 16 Lines	if (vchiq_init_state(state, vchiq_slot_zero, 0/slave/) !=
VCHIQ_SUCCESS) {		VCHIQ_SUCCESS) {
err = -EINVAL;		err = -EINVAL;
goto failed_vchiq_init;		goto failed_vchiq_init;
}		}

bcm_mbox_write(BCM2835_MBOX_CHAN_VCHIQ, (unsigned int)g_slot_phys);		bcm_mbox_write(BCM2835_MBOX_CHAN_VCHIQ, (unsigned int)g_slot_phys);

vchiq_log_info(vchiq_arm_log_level,		vchiq_log_info(vchiq_arm_log_level,
"vchiq_init - done (slots %x, phys %x)",		"vchiq_init - done (slots %zx, phys %zx)",
(unsigned int)vchiq_slot_zero, g_slot_phys);		(size_t)vchiq_slot_zero, g_slot_phys);

vchiq_call_connected_callbacks();		vchiq_call_connected_callbacks();

return 0;		return 0;

failed_vchiq_init:		failed_vchiq_init:
failed_init_slots:		failed_init_slots:
bus_dmamap_unload(bcm_slots_dma_tag, bcm_slots_dma_map);		bus_dmamap_unload(bcm_slots_dma_tag, bcm_slots_dma_map);
▲ Show 20 Lines • Show All 169 Lines • ▼ Show 20 Lines
** cached area.		** cached area.

** N.B. This implementation plays slightly fast and loose with the Linux		** N.B. This implementation plays slightly fast and loose with the Linux
** driver programming rules, e.g. its use of __virt_to_bus instead of		** driver programming rules, e.g. its use of __virt_to_bus instead of
** dma_map_single, but it isn't a multi-platform driver and it benefits		** dma_map_single, but it isn't a multi-platform driver and it benefits
** from increased speed as a result.		** from increased speed as a result.
*/		*/


static int		static int
create_pagelist(char __user *buf, size_t count, unsigned short type,		create_pagelist(char __user *buf, size_t count, unsigned short type,
struct proc p, BULKINFO_T bi)		struct proc p, BULKINFO_T bi)
{		{
PAGELIST_T *pagelist;		PAGELIST_T *pagelist;
vm_page_t* pages;		vm_page_t* pages;
unsigned long *addrs;		uint32_t *addrs;
unsigned int num_pages, i;		unsigned int num_pages, i;
vm_offset_t offset;		vm_offset_t offset;
int pagelist_size;		int pagelist_size;
char addr, base_addr, *next_addr;		char addr, base_addr, *next_addr;
int run, addridx, actual_pages;		int run, addridx, actual_pages;
int err;		int err;
vm_paddr_t pagelist_phys;		vm_paddr_t pagelist_phys;
vm_paddr_t pa;		vm_paddr_t pa;
Show All 20 Lines	err = bus_dma_tag_create(
NULL, NULL, /* filter, filterarg */		NULL, NULL, /* filter, filterarg */
pagelist_size, 1, /* maxsize, nsegments */		pagelist_size, 1, /* maxsize, nsegments */
pagelist_size, 0, /* maxsegsize, flags */		pagelist_size, 0, /* maxsegsize, flags */
NULL, NULL, /* lockfunc, lockarg */		NULL, NULL, /* lockfunc, lockarg */
&bi->pagelist_dma_tag);		&bi->pagelist_dma_tag);

err = bus_dmamem_alloc(bi->pagelist_dma_tag, (void **)&pagelist,		err = bus_dmamem_alloc(bi->pagelist_dma_tag, (void **)&pagelist,
BUS_DMA_COHERENT \| BUS_DMA_WAITOK, &bi->pagelist_dma_map);		BUS_DMA_COHERENT \| BUS_DMA_WAITOK, &bi->pagelist_dma_map);
if (err) {		if (err \|\| !pagelist) {
vchiq_log_error(vchiq_core_log_level, "Unable to allocate pagelist memory");		vchiq_log_error(vchiq_core_log_level, "Unable to allocate pagelist memory");
err = -ENOMEM;		err = -ENOMEM;
goto failed_alloc;		goto failed_alloc;
}		}

err = bus_dmamap_load(bi->pagelist_dma_tag, bi->pagelist_dma_map, pagelist,		err = bus_dmamap_load(bi->pagelist_dma_tag, bi->pagelist_dma_map, pagelist,
pagelist_size, vchiq_dmamap_cb,		pagelist_size, vchiq_dmamap_cb,
&pagelist_phys, 0);		&pagelist_phys, 0);

if (err) {		if (err) {
vchiq_log_error(vchiq_core_log_level, "cannot load DMA map for pagelist memory");		vchiq_log_error(vchiq_core_log_level, "cannot load DMA map for pagelist memory");
err = -ENOMEM;		err = -ENOMEM;
		bi->pagelist = pagelist;
goto failed_load;		goto failed_load;
}		}

vchiq_log_trace(vchiq_arm_log_level,		vchiq_log_trace(vchiq_arm_log_level,
"create_pagelist - %x (%d bytes @%p)", (unsigned int)pagelist, count, buf);		"create_pagelist - %zx (%zu bytes @%p)", (size_t)pagelist, count, buf);

if (!pagelist)
return -ENOMEM;

addrs = pagelist->addrs;		addrs = pagelist->addrs;
pages = (vm_page_t*)(addrs + num_pages);		pages = (vm_page_t*)(addrs + num_pages);

actual_pages = vm_fault_quick_hold_pages(&p->p_vmspace->vm_map,		actual_pages = vm_fault_quick_hold_pages(&p->p_vmspace->vm_map,
(vm_offset_t)buf, count,		(vm_offset_t)buf, count,
(type == PAGELIST_READ ? VM_PROT_WRITE : 0 ) \| VM_PROT_READ, pages, num_pages);		(type == PAGELIST_READ ? VM_PROT_WRITE : 0 ) \| VM_PROT_READ, pages, num_pages);

if (actual_pages != num_pages) {		if (actual_pages != num_pages) {
if (actual_pages > 0)		if (actual_pages > 0)
vm_page_unhold_pages(pages, actual_pages);		vm_page_unhold_pages(pages, actual_pages);
free(pagelist, M_VCPAGELIST);		err = -ENOMEM;
return (-ENOMEM);		bi->pagelist = pagelist;
		goto failed_hold;
}		}

pagelist->length = count;		pagelist->length = count;
pagelist->type = type;		pagelist->type = type;
pagelist->offset = offset;		pagelist->offset = offset;

/* Group the pages into runs of contiguous pages */		/* Group the pages into runs of contiguous pages */

base_addr = (void *)PHYS_TO_VCBUS(VM_PAGE_TO_PHYS(pages[0]));		size_t run_ceil = g_long_bulk_space ? 0x100 : PAGE_SIZE;
		unsigned int pg_addr_rshift = g_long_bulk_space ? 4 : 0;
		base_addr = (void *) VM_PAGE_TO_VC_BULK_PAGE(pages[0]);
next_addr = base_addr + PAGE_SIZE;		next_addr = base_addr + PAGE_SIZE;
addridx = 0;		addridx = 0;
run = 0;		run = 0;
		#define _PG_BLOCK(base,run) \
		((((size_t) (base)) >> pg_addr_rshift) & ~(run_ceil-1)) + (run)
for (i = 1; i < num_pages; i++) {		for (i = 1; i < num_pages; i++) {
addr = (void *)PHYS_TO_VCBUS(VM_PAGE_TO_PHYS(pages[i]));		addr = (void *)VM_PAGE_TO_VC_BULK_PAGE(pages[i]);
if ((addr == next_addr) && (run < (PAGE_SIZE - 1))) {		if ((addr == next_addr) && (run < run_ceil - 1)) {
next_addr += PAGE_SIZE;		next_addr += PAGE_SIZE;
run++;		run++;
} else {		} else {
addrs[addridx] = (unsigned long)base_addr + run;		addrs[addridx++] = (uint32_t) _PG_BLOCK(base_addr,run);
addridx++;
base_addr = addr;		base_addr = addr;
next_addr = addr + PAGE_SIZE;		next_addr = addr + PAGE_SIZE;
run = 0;		run = 0;
}		}
}		}
		addrs[addridx++] = _PG_BLOCK(base_addr, run);
		#undef _PG_BLOCK

addrs[addridx] = (unsigned long)base_addr + run;
addridx++;

/* Partial cache lines (fragments) require special measures */		/* Partial cache lines (fragments) require special measures */
if ((type == PAGELIST_READ) &&		if ((type == PAGELIST_READ) &&
((pagelist->offset & (g_cache_line_size - 1)) \|\|		((pagelist->offset & (g_cache_line_size - 1)) \|\|
((pagelist->offset + pagelist->length) &		((pagelist->offset + pagelist->length) &
(g_cache_line_size - 1)))) {		(g_cache_line_size - 1)))) {
char *fragments;		char *fragments;

if (down_interruptible(&g_free_fragments_sema) != 0) {		if (down_interruptible(&g_free_fragments_sema) != 0) {
free(pagelist, M_VCPAGELIST);		free(pagelist, M_VCPAGELIST);
return -EINTR;		return -EINTR;
}		}

WARN_ON(g_free_fragments == NULL);		WARN_ON(g_free_fragments == NULL);

down(&g_free_fragments_mutex);		down(&g_free_fragments_mutex);
fragments = g_free_fragments;		fragments = g_free_fragments;
WARN_ON(fragments == NULL);		WARN_ON(fragments == NULL);
g_free_fragments = (char *) g_free_fragments;		g_free_fragments = (char *) g_free_fragments;
up(&g_free_fragments_mutex);		up(&g_free_fragments_mutex);
pagelist->type =		pagelist->type =
PAGELIST_READ_WITH_FRAGMENTS +		PAGELIST_READ_WITH_FRAGMENTS
(fragments - g_fragments_base)/g_fragment_size;		+ (fragments - g_fragments_base)/g_fragment_size;
		#if defined(__aarch64__)
		bus_dmamap_sync(bcm_slots_dma_tag, bcm_slots_dma_map, BUS_DMASYNC_PREREAD);
		#endif
}		}

		#if defined(__aarch64__)
		if(type == PAGELIST_READ) {
		cpu_dcache_wbinv_range(buf, count);
		} else {
		cpu_dcache_wb_range(buf, count);
		}
		dsb(sy);
		#else
pa = pmap_extract(PCPU_GET(curpmap), (vm_offset_t)buf);		pa = pmap_extract(PCPU_GET(curpmap), (vm_offset_t)buf);
dcache_wbinv_poc((vm_offset_t)buf, pa, count);		dcache_wbinv_poc((vm_offset_t)buf, pa, count);
		#endif

bus_dmamap_sync(bi->pagelist_dma_tag, bi->pagelist_dma_map, BUS_DMASYNC_PREWRITE);		bus_dmamap_sync(bi->pagelist_dma_tag, bi->pagelist_dma_map, BUS_DMASYNC_PREWRITE);

bi->pagelist = pagelist;		bi->pagelist = pagelist;

return 0;		return 0;

		failed_hold:
		bus_dmamap_unload(bi->pagelist_dma_tag,bi->pagelist_dma_map);
failed_load:		failed_load:
bus_dmamem_free(bi->pagelist_dma_tag, bi->pagelist, bi->pagelist_dma_map);		bus_dmamem_free(bi->pagelist_dma_tag, bi->pagelist, bi->pagelist_dma_map);
failed_alloc:		failed_alloc:
bus_dma_tag_destroy(bi->pagelist_dma_tag);		bus_dma_tag_destroy(bi->pagelist_dma_tag);

return err;		return err;
}		}

static void		static void
free_pagelist(BULKINFO_T *bi, int actual)		free_pagelist(BULKINFO_T *bi, int actual)
{		{
vm_page_t*pages;		vm_page_t*pages;
unsigned int num_pages, i;		unsigned int num_pages, i;
PAGELIST_T *pagelist;		PAGELIST_T *pagelist;

pagelist = bi->pagelist;		pagelist = bi->pagelist;

vchiq_log_trace(vchiq_arm_log_level,		vchiq_log_trace(vchiq_arm_log_level,
"free_pagelist - %x, %d (%lu bytes @%p)", (unsigned int)pagelist, actual, pagelist->length, bi->buf);		"free_pagelist - %zx, %d (%u bytes @%p)", (size_t)pagelist, actual, pagelist->length, bi->buf);

num_pages =		num_pages =
(pagelist->length + pagelist->offset + PAGE_SIZE - 1) /		(pagelist->length + pagelist->offset + PAGE_SIZE - 1) /
PAGE_SIZE;		PAGE_SIZE;

pages = (vm_page_t*)(pagelist->addrs + num_pages);		pages = (vm_page_t*)(pagelist->addrs + num_pages);

		#if defined(__aarch64__)
		/*
		* On arm64, even if the user keeps their end of the bargain
		* -- do NOT touch the buffers sent to VC -- but reads around the
		* pagelist after the invalidation above, the arm might preemptively
		* load (and validate) cache lines for areas inside the page list,
		* so we must invalidate them again.
		*
		* The functional test does it and without this it doesn't pass.
		*
		* XXXMDC might it be enough to invalidate a couple of pages at
		* the ends of the page list?
		*/
		if(pagelist->type >= PAGELIST_READ && actual > 0)
		invalidate_cachelines_in_range_of_ppage_seq(
		pages,
		pagelist->offset,
		actual
		);
		#endif

/* Deal with any partial cache lines (fragments) */		/* Deal with any partial cache lines (fragments) */
if (pagelist->type >= PAGELIST_READ_WITH_FRAGMENTS) {		if (pagelist->type >= PAGELIST_READ_WITH_FRAGMENTS) {
char *fragments = g_fragments_base +		char *fragments = g_fragments_base +
(pagelist->type - PAGELIST_READ_WITH_FRAGMENTS)*g_fragment_size;		(pagelist->type - PAGELIST_READ_WITH_FRAGMENTS)*g_fragment_size;
int head_bytes, tail_bytes;		int head_bytes, tail_bytes;
head_bytes = (g_cache_line_size - pagelist->offset) &		head_bytes = (g_cache_line_size - pagelist->offset) &
(g_cache_line_size - 1);		(g_cache_line_size - 1);
tail_bytes = (pagelist->offset + actual) &		tail_bytes = (pagelist->offset + actual) &
Show All 20 Lines	if (pagelist->type >= PAGELIST_READ_WITH_FRAGMENTS) {

down(&g_free_fragments_mutex);		down(&g_free_fragments_mutex);
(char *) fragments = g_free_fragments;		(char *) fragments = g_free_fragments;
g_free_fragments = fragments;		g_free_fragments = fragments;
up(&g_free_fragments_mutex);		up(&g_free_fragments_mutex);
up(&g_free_fragments_sema);		up(&g_free_fragments_sema);
}		}

for (i = 0; i < num_pages; i++) {
if (pagelist->type != PAGELIST_WRITE) {		if (pagelist->type != PAGELIST_WRITE) {
		for (i = 0; i < num_pages; i++) {
vm_page_dirty(pages[i]);		vm_page_dirty(pages[i]);
pagelist_page_free(pages[i]);		pagelist_page_free(pages[i]);
}		}
}		}

		#if defined(__aarch64__)
		/* XXXMDC necessary? */
		dsb(sy);
		#endif

bus_dmamap_unload(bi->pagelist_dma_tag, bi->pagelist_dma_map);		bus_dmamap_unload(bi->pagelist_dma_tag, bi->pagelist_dma_map);
bus_dmamem_free(bi->pagelist_dma_tag, bi->pagelist, bi->pagelist_dma_map);		bus_dmamem_free(bi->pagelist_dma_tag, bi->pagelist, bi->pagelist_dma_map);
bus_dma_tag_destroy(bi->pagelist_dma_tag);		bus_dma_tag_destroy(bi->pagelist_dma_tag);

free(bi, M_VCPAGELIST);		free(bi, M_VCPAGELIST);
}		}