Is there any example code to map PCI registers into user space? I am having
problem getting functions rtdm_iomap_to_user and mmap to compile. My code
is attached.
Xenomai - 2.4.10
Kernel - 2.6.29.1
OS - Red Hat 5.5
Thanks in advance,
Peter Hua
/**
* This a driver written to interface with DSP boards from Innovative Integration
* It utilizes the RTDM device APIs from the Xenomai realtime Linux project.
*
*/
#include <linux/version.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/init.h>
#include <linux/pci.h>
#include <linux/interrupt.h>
#include <linux/delay.h>
#include <linux/workqueue.h>
#include <linux/reboot.h>
#include <linux/slab.h>
#include <linux/cdev.h>
#include <linux/spinlock.h>
#include <linux/pagemap.h>
#include <linux/semaphore.h>
#include <linux/list.h>
#include <linux/vmalloc.h>
#include <linux/mman.h>
#include <asm/uaccess.h>
#include <asm/dma.h>
#include <asm/delay.h>
#include <asm/io.h>
#include <asm/msr.h>
#include <asm/page.h>
#include <linux/mman.h>
#include <asm-generic/system.h>
#include <rtdm/rtdm_driver.h>
#include "malibu.h"
#define VENDOR_ID 0x1303
#define DRV_NAME "malibu"
#define SIZE_MAX 1024
#define SOME_SUB_CLASS 1303
static void errMsg(int line, const char *place, const char *msg)
{
rtdm_printk(KERN_ALERT "malibu: Error at %s() line: %d, %s\n", place, line,
msg);
}
#define MY_ERR(s) errMsg(__LINE__, __FUNCTION__, s)
static void logMsg(int line, const char *place, const char *msg)
{
rtdm_printk(KERN_INFO "MalibuDrv_RT: %s() line: %d, %s\n", place, line,
msg);
}
#define MY_PRINT(s) logMsg(__LINE__, __FUNCTION__, s);
typedef struct {
void* kAddr;
uint64_t start;
uint64_t end;
uint64_t size;
} MyBar;
typedef struct {
void* kAddr;
dma_addr_t physAddr;
uint32_t size;
} MyBuf;
typedef struct {
struct list_head list;
struct cdev cdev;
struct device *sysdev;
struct pci_dev *pdev;
dev_t myDevT;
MyBar bars[MALIBU_BAR_MAX];
MyBuf bufs[MALIBU_BUF_MAX]; //DMA buffers
wait_queue_head_t intrWait;
struct fasync_struct* async_queue;
int mySig;
void* intrStatusAddr;
u32 intrMask;
u8 intrWriteback;
u32 iQueue[MALIBU_INTR_MAX];
u32 iPtrLow;
u32 iPtrHi;
void (*cntxtFunc)(void*);
void *cntxtArg;
u16 devOpen;
u8 wrOpen;
u8 numBars;
u8 isIrqEnabled;
int dev_id;
struct rtdm_device *xen_rtdm_device;
rtdm_irq_t irq_handle; /* device IRQ handle */
} MyData;
static struct class* devClass;
static dev_t baseDevT;
static int devCnt = 0;
LIST_HEAD(devList);
static int fasync(int fd, struct file *filep, int mode);
static int malibu_intr_handler(rtdm_irq_t * irq_context);
static char msgbuf[1000];
int MyMmap(struct file *filep, struct vm_area_struct *vma);
static struct file_operations myFileOps = {
.owner = THIS_MODULE,
//.release = release,
.mmap = MyMmap,
//.fasync = fasync,
};
static int malibu_rtdm_open(struct rtdm_dev_context *context,
rtdm_user_info_t * user_info, int oflags)
//static int open(struct inode *inodep, struct file *filep)
{
MyData *md, *ctx;
bool found=false;
int err=0;
unsigned long vaddr;
MY_PRINT("start");
ctx = (MyData *)context->dev_private;
list_for_each_entry(md, &devList, list) {
if (md->dev_id == context->device->device_id)
{
found = true;
MY_PRINT("device FOUND");
sprintf (msgbuf, "md=%p, md->device_id=%d", md, md->dev_id);
MY_PRINT(msgbuf);
memcpy(ctx, md, sizeof(MyData));
break;
}
}
if(!found)
{
MY_ERR("no matching device");
return -ENODEV;
}
if (ctx->wrOpen && (oflags & FMODE_WRITE)) {
MY_ERR("device already opened");
return -EBUSY;
}
#if 0
/* mark pages reserved so that remap_pfn_range works */
int b;
for(b=0;b<MALIBU_BAR_MAX;b++)
{
if (ctx->bars[b].kAddr == NULL)
break;
for (vaddr = (unsigned long)ctx->bars[b].kAddr;
vaddr < (unsigned long)ctx->bars[b].kAddr + ctx->bars[b].size;
vaddr += PAGE_SIZE)
SetPageReserved(virt_to_page(vaddr));
}
sprintf(msgbuf, "reserved, b=%d", b);
MY_PRINT(msgbuf);
#endif
if (oflags & FMODE_WRITE)
{
ctx->iPtrLow = 0;
ctx->iPtrHi = 0;
ctx->mySig = SIGIO;
ctx->cntxtFunc = 0;
ctx->cntxtArg = 0;
static unsigned long irqtype = (RTDM_IRQTYPE_SHARED |
RTDM_IRQTYPE_EDGE);
sprintf(msgbuf, "irqtype=%d", irqtype);
MY_PRINT(msgbuf);
err = rtdm_irq_request(&ctx->irq_handle, ctx->pdev->irq,
(rtdm_irq_handler_t)malibu_intr_handler,
irqtype,
context->device->proc_name, (void *)ctx);
if(err != 0)
{
sprintf(msgbuf, "rtdm_irq_request() failed, err=%d, irq=%d,
proc_name=%s",
err, ctx->pdev->irq, context->device->proc_name);
MY_ERR(msgbuf);
return err;
}
sprintf(msgbuf, "rtdm_irq_request() OK, irq=%d", ctx->irq_handle);
MY_PRINT(msgbuf);
ctx->wrOpen = 1;
}
ctx->devOpen++;
MY_PRINT("end OK");
return 0;
}
/**
* Close the device
*
* This function is called when the device shall be closed.
*
*/
static int malibu_rtdm_close(struct rtdm_dev_context *context,
rtdm_user_info_t * user_info)
{
MyData *ctx = context->dev_private;
rtdm_irq_free(&ctx->irq_handle);
MY_PRINT("closing device");
return 0;
}
static void freeBuffer(MyData* md, int n)
{
MyBuf* b = &md->bufs[n];
if (b->size == 0) return;
dma_free_coherent(&md->pdev->dev, b->size, b->kAddr, b->physAddr);
b->size = 0;
}
static int release(struct inode *inode, struct file *filep)
{
int i;
MyData* md = (MyData*) filep->private_data;
md->devOpen--;
if (filep->f_mode & FMODE_WRITE) {
md->wrOpen = 0;
//free_irq(md->pdev->irq, md);
rtdm_irq_free(&md->irq_handle);
for (i = 0; i < MALIBU_BUF_MAX; i++) freeBuffer(md, i);
fasync(-1, filep, 0);
}
return 0;
}
/* --- Mmap functions --- */
void malibu_map(struct vm_area_struct *area)
{
//unsigned long *status = (unsigned long *)area->vm_private_data;
//set_bit(MALIBU_BUF_MAP_NR, status);
}
void malibu_unmap(struct vm_area_struct *area)
{
//unsigned long *status = (unsigned long *)area->vm_private_data;
//clear_bit(MALIBU_BUF_MAP_NR, status);
}
static struct vm_operations_struct malibu_vm_ops = {
.open = malibu_map,
.close = malibu_unmap,
};
static struct vm_area_struct vma;
void * malibu_ioctl_mmap(rtdm_user_info_t *user_info,
MyData *cxt, MalibuDrv_MmapArg *arg)
{
//malibu_mmap_t map_cfg;
//malibu_dev_t *dev;
//malibu_buf_t *buf;
int ret=0;
void *usrAddr;
MY_PRINT("start");
/* Basic checkings */
#if 0
/* The mmap operation cannot be performed in a
real-time context */
if (rtdm_in_rt_context()) {
MY_ERR("in real-time context");
return -ENOSYS;
}
// dev = malibu_get_dev(cxt);
// buf = cxt->buffer;
if (!test_bit(malibu_DEV_ATTACHED_NR, &dev->flags)) {
__malibu_err("malibu_ioctl_mmap: cannot mmap on "
"an unattached device\n");
return -EINVAL;
}
if (test_bit(malibu_BUF_MAP_NR, &buf->flags)) {
__malibu_err("malibu_ioctl_mmap: buffer already mapped\n");
return -EBUSY;
}
if (rtdm_safe_copy_from_user(cxt->user_info,
&map_cfg, arg, sizeof(malibu_mmap_t)) != 0)
return -EFAULT;
/* Check the size to be mapped */
if (arg->size > cxt->bars[arg->barnum].size)
{
MY_ERR("bar size exceeded");
return -EFAULT;
}
#endif
/* All the magic is here */
sprintf(msgbuf, "kAddr=0x%p, size=%d", cxt->bars[arg->barnum].kAddr,
arg->size);
MY_PRINT(msgbuf);
//arg->size += (PAGE_SIZE - (arg->size % PAGE_SIZE));
struct file *filp;
struct file_operations *old_fops;
void *old_priv_data;
void *user_addr;
filp = filp_open("/dev/zero", O_RDWR, 0);
if (IS_ERR(filp)) {
printk("open failed\n");
return filp;
}
old_fops = filp->f_op;
filp->f_op = &myFileOps;
old_priv_data = filp->private_data;
filp->private_data = cxt;
//down_write(¤t->mm->mmap_sem);
usrAddr = (void *)mmap(
0,
(size_t)arg->size,
(PROT_READ|PROT_WRITE),
MAP_SHARED,
filp, 0);
//up_write(¤t->mm->mmap_sem);
filp->f_op = old_fops;
filp->private_data = old_priv_data;
filp_close(filp, current->files);
#if 0
ret = rtdm_iomap_to_user(user_info,
cxt->bars[arg->barnum].kAddr,
arg->size,
PROT_READ | PROT_WRITE,
&usrAddr, //&arg->userAddr,
&malibu_vm_ops,
NULL);
#endif
if (ret < 0) {
sprintf(msgbuf, "malibu_ioctl_mmap: internal error, "
"rtdm_mmap_to_user failed (err=%d)\n", ret);
MY_ERR(msgbuf);
}
sprintf(msgbuf, "usrAddr=%p", usrAddr);
MY_PRINT(msgbuf);
//rtdm_safe_copy_to_user(cxt->user_info,
// &arg->UsrAdr, &usrAddr, sizeof(unsigned int));
MY_PRINT("end");
return usrAddr;
}
//static int ioctl(struct inode *inodep, struct file *filep, unsigned int cmd,
unsigned long arg)
int malibu_rtdm_ioctl(struct rtdm_dev_context *context,
rtdm_user_info_t *user_info,
int cmd,
void *arg)
{
MyData* md = (MyData *)context->dev_private;
void *addr;
int err;
int retval = 0;
ioc_param_struct param = {0,0};
if (_IOC_TYPE(cmd) != MALIBU_IOC_MAGIC) return -ENOTTY;
/*
* the direction is a bitmask, and VERIFY_WRITE catches R/W
* transfers. `Type' is user-oriented, while
* access_ok is kernel-oriented, so the concept of "read" and
* "write" is reversed (i.e. when an user wants to read data,
* the kernel has to write them into the user space)
*
* always check the user space address before accessing it
*
* access_ok returns 0 for failure!
*/
if (_IOC_DIR(cmd) & _IOC_READ)
err = !rtdm_read_user_ok(user_info, arg, sizeof(ioc_param_struct));
else if (_IOC_DIR(cmd) & _IOC_WRITE)
err = !rtdm_rw_user_ok(user_info, arg, sizeof(ioc_param_struct));
if (err) return -EFAULT;
switch(cmd) {
case MALIBU_MMAP_BLOCK: {
void * usrAddr;
MalibuDrv_MmapArg *mArg = (MalibuDrv_MmapArg *)arg;
rtdm_iomap_to_user(user_info,
mArg->kernAddr,
mArg->size,
PROT_READ | PROT_WRITE,
&usrAddr, //&arg->userAddr,
&malibu_vm_ops,
NULL);
mArg->userAddr = usrAddr;
return (usrAddr);
break;
}
case MALIBU_MMAP_BAR: {
addr = malibu_ioctl_mmap(user_info,
md, arg);
MalibuDrv_MmapArg *tmp = (MalibuDrv_MmapArg *)(arg);
tmp->userAddr = addr;
sprintf(msgbuf, "addr=%p, addr=%d", tmp->userAddr,
(int)addr);
MY_PRINT(msgbuf);
return (int)addr;
break;
}
case MALIBU_GET_INFO: {
MalibuDrv_INFO x;
x.vendorId = md->pdev->vendor;
x.deviceId = md->pdev->device;
x.subVendorId = md->pdev->subsystem_vendor;
x.subDeviceId = md->pdev->subsystem_device;
x.revision = md->pdev->revision;
x.bus = md->pdev->bus->number;
x.slot = PCI_SLOT(md->pdev->devfn);
x.func = PCI_FUNC(md->pdev->devfn);
x.numBars = md->numBars;
MY_PRINT("copying data to user");
sprintf(msgbuf, "x.vendorId=0x%x, x.deviceId=0x%x,
x.numBars=%d",
md->pdev->vendor, md->pdev->device, x.numBars);
MY_PRINT(msgbuf);
if (user_info)
err = rtdm_safe_copy_to_user(user_info, arg,
&x, sizeof(x));
else
memcpy(arg, &x, sizeof(MalibuDrv_INFO));
break;
return err;
}
case MALIBU_GET_BAR_SIZE:
if (*(int *)arg < 0 || *(int *)arg >= MALIBU_BAR_MAX)
return -EINVAL;
sprintf(msgbuf, "arg=%p, *arg=%d", arg, *(int *)arg);
MY_PRINT(msgbuf);
return md->bars[*(int *)arg].size;
case MALIBU_ALLOC_BUFFER: {
int n;
MalibuDrv_Buffer x;
MyBuf* b;
MY_PRINT("MALIBU_ALLOC_BUFFER start");
//TODO: if (!(filep->f_mode & FMODE_WRITE)) return -EACCES;
for (n = 0; n < MALIBU_BUF_MAX; n++) {
if (md->bufs[n].size == 0) break;
}
MY_PRINT("1");
if (n == MALIBU_BUF_MAX) {
MY_ERR("MALIBU_BUF_MAX");
return -ENOMEM;
}
MY_PRINT("2");
if (rtdm_safe_copy_from_user(user_info, &x, (const void
__user*)arg, sizeof(x)))
{
MY_ERR("rtdm_safe_copy_from_user");
return -EFAULT;
}
MY_PRINT("3");
b = &md->bufs[n];
b->size = ((x.size + PAGE_SIZE - 1) / PAGE_SIZE) *
PAGE_SIZE;
b->kAddr = dma_alloc_coherent(&md->pdev->dev, b->size,
&b->physAddr, GFP_KERNEL);
MY_PRINT("4");
if (b->kAddr == 0) {
b->size = 0;
MY_ERR("b->kAddr == 0");
return -ENOMEM;
}
MY_PRINT("5");
x.physAddr = b->physAddr;
x.bufNum = n;
x.fileOffset = (n + MALIBU_BAR_MAX) * MALIBU_MMAP_SIZE;
sprintf(msgbuf, "x.pa=0x%p",x.physAddr);
MY_PRINT(msgbuf);
if (user_info)
err = rtdm_safe_copy_to_user(user_info, arg,
&x, sizeof(x));
else
memcpy(arg, &x, sizeof(x));
break;
MY_PRINT("7");
if(err != 0)
{
MY_ERR("rtdm_safe_copy_to_user");
freeBuffer(md, x.bufNum);
return err;
}
MY_PRINT("8");
((uint32_t*)b->kAddr)[0] = 0x12345678;
((uint32_t*)b->kAddr)[1] = n;
((uint32_t*)b->kAddr)[2] = md->myDevT;
MY_PRINT("MALIBU_ALLOC_BUFFER end OK");
return 0;
}
case MALIBU_FREE_BUFFER: {
int i;
MalibuDrv_Buffer x;
//TODO:if (!(filep->f_mode & FMODE_WRITE)) return -EACCES;
if (rtdm_safe_copy_from_user(user_info, &x, (const void
__user*)arg, sizeof(x))) return -EFAULT;
for (i = 0; i < MALIBU_BUF_MAX; i++) {
if (md->bufs[i].physAddr == x.physAddr) {
freeBuffer(md, i);
break;
}
}
if (i == MALIBU_BUF_MAX) {
MY_ERR("");
return -ENOMEM;
}
return 0;
}
case MALIBU_ISR_ENABLE:
//TODO: if (!(filep->f_mode & FMODE_WRITE)) return -EACCES;
//enable_irq(md->pdev->irq);
rtdm_irq_enable(&md->irq_handle);
md->isIrqEnabled = 1;
return 0;
case MALIBU_ISR_DISABLE:
//TODO: if (!(filep->f_mode & FMODE_WRITE)) return -EACCES;
md->isIrqEnabled = 0;
//disable_irq(md->pdev->irq);
rtdm_irq_disable(&md->irq_handle);
return 0;
case MALIBU_GET_ISR_ENABLED:
return md->isIrqEnabled;
case MALIBU_GET_ISR_DEPTH:
return (md->iPtrLow - md->iPtrHi);
case MALIBU_ISR_CLEAR_ALL:
//TODO: if (!(filep->f_mode & FMODE_WRITE)) return -EACCES;
//disable_irq(md->pdev->irq);
rtdm_irq_disable(&md->irq_handle);
md->iPtrLow = 0;
md->iPtrHi = 0;
//enable_irq(md->pdev->irq);
rtdm_irq_enable(&md->irq_handle);
return 0; // no implemented yet
case MALIBU_DEQUEUE_ISR: {
MalibuDrv_DequeueISR x;
unsigned long j;
int ret;
//TODO: if (!(filep->f_mode & FMODE_WRITE)) return -EACCES;
if (rtdm_safe_copy_from_user(user_info, &x, (const void
__user*)arg, (caddr_t)&x.depth - (caddr_t)&x)) return -EFAULT;
x.depth = 0;
x.cntxtFunc = md->cntxtFunc;
x.cntxtArg = md->cntxtArg;
x.errno = 0;
x.depth = 0;
j = timeval_to_jiffies(&x.timeout);
if (j == 0) {
if (md->iPtrLow != md->iPtrHi) {
while (md->iPtrLow != md->iPtrHi && x.depth
< MALIBU_INTR_MAX) {
x.data[x.depth++] =
md->iQueue[md->iPtrHi % MALIBU_INTR_MAX];
md->iPtrHi++;
}
}
else {
x.errno = ENODATA;
}
}
else {
ret =
wait_event_interruptible_timeout(md->intrWait, md->iPtrLow != md->iPtrHi, j);
if (ret == 0) x.errno = ETIME;
else if (ret == -ERESTARTSYS) x.errno = ERESTARTSYS;
else {
while (md->iPtrLow != md->iPtrHi && x.depth
< MALIBU_INTR_MAX) {
x.data[x.depth++] =
md->iQueue[md->iPtrHi % MALIBU_INTR_MAX];
md->iPtrHi++;
}
jiffies_to_timeval(ret, &x.timeout);
}
}
if (user_info)
err = rtdm_safe_copy_to_user(user_info, arg,
&x,
(caddr_t)&x.data[x.depth] - (caddr_t)&x);
else
memcpy(arg, &x, (caddr_t)&x.data[x.depth] -
(caddr_t)&x);
break;
return err;
}
case MALIBU_SET_ISR_CTRL: {
MalibuDrv_IntCtrl x;
//TODO: if (!(filep->f_mode & FMODE_WRITE)) return -EACCES;
if (rtdm_safe_copy_from_user(user_info, &x, (const void
__user*)arg, sizeof(x))) return -EFAULT;
md->intrMask = x.mask;
md->intrWriteback = x.type;
if (x.bar > MALIBU_BAR_MAX) {
MY_ERR("");
return -EINVAL;
}
if (x.byteOffset > md->bars[x.bar].size) {
MY_ERR("");
return -EINVAL;
}
md->intrStatusAddr = md->bars[x.bar].kAddr + x.byteOffset;
md->cntxtFunc = x.cntxtFunc;
md->cntxtArg = x.cntxtArg;
return 0;
}
case MALIBU_SET_SIG:
//TODO: if (!(filep->f_mode & FMODE_WRITE)) return -EACCES;
md->mySig = arg;
return 0;
case MALIBU_FAKE_SIG: {
MalibuDrv_FakeISR x;
if (rtdm_safe_copy_from_user(user_info, &x, (const void
__user*)arg, sizeof(x))) return -EFAULT;
if ((md->iPtrLow - md->iPtrHi) < MALIBU_INTR_MAX-x.depth-1)
{
int i;
for (i = 0; i < x.depth; i++) {
md->iQueue[md->iPtrLow % MALIBU_INTR_MAX] =
x.data[i];
md->iPtrLow++;
}
wake_up_interruptible(&md->intrWait);
if (md->async_queue) kill_fasync(&md->async_queue,
md->mySig, POLL_IN);
}
return 0;
}
default:
return -EINVAL;
}
}
//static irqreturn_t board_handler(unsigned int irq, void *dev_id, struct
pt_regs *regs)
static int malibu_intr_handler(rtdm_irq_t * irq_context)
{
uint32_t rd;
MyData *md = rtdm_irq_get_arg(irq_context, MyData);
//MyData* md = (MyData*) dev_id;
//if(md->pdev->irq != irq) return IRQ_NONE;
rd = ioread32(md->intrStatusAddr);
if ((rd & md->intrMask) == 0) return IRQ_NONE;
if ((md->iPtrLow - md->iPtrHi) < MALIBU_INTR_MAX-1) {
md->iQueue[md->iPtrLow % MALIBU_INTR_MAX] = rd;
md->iPtrLow++;
wake_up_interruptible(&md->intrWait);
if (md->async_queue && md->mySig) kill_fasync(&md->async_queue,
md->mySig, POLL_IN);
}
if (md->intrWriteback) iowrite32(rd, md->intrStatusAddr);
return IRQ_HANDLED;
}
static void vma_open(struct vm_area_struct* vma) { }
static void vma_close(struct vm_area_struct* vma) { }
static struct vm_operations_struct remap_vm_ops = {
.open = vma_open,
.close = vma_close,
};
int MyMmap(struct file *filep, struct vm_area_struct *vma)
{
MY_PRINT("start");
MyData* md = (MyData*) filep->private_data;
unsigned long off = vma->vm_pgoff << PAGE_SHIFT;
unsigned long vsize = vma->vm_end - vma->vm_start;
unsigned long seg = off / MALIBU_MMAP_SIZE;
unsigned long seg_off = off % MALIBU_MMAP_SIZE;
unsigned long psize, pfn;
if (seg >= 0 && seg < MALIBU_BAR_MAX) {
if (md->bars[seg].size == 0) return -EINVAL;
pfn = (md->bars[seg].start + seg_off) >> PAGE_SHIFT;
psize = md->bars[seg].size - seg_off;
}
else if (seg >= MALIBU_BAR_MAX && seg < MALIBU_BAR_MAX + MALIBU_BUF_MAX) {
seg -= MALIBU_BAR_MAX;
if (md->bufs[seg].size == 0) return -EINVAL;
pfn = __pa(md->bufs[seg].kAddr + seg_off) >> PAGE_SHIFT;
psize = md->bufs[seg].size - seg_off;
}
else return -EINVAL;
if (vsize > psize) return -EINVAL;
if (remap_pfn_range(vma, vma->vm_start, pfn, vsize, vma->vm_page_prot))
return -EAGAIN;
vma->vm_ops = &remap_vm_ops;
vma_open(vma);
MY_PRINT("end");
return 0;
}
static int fasync(int fd, struct file *filep, int mode)
{
MyData* md = (MyData*) filep->private_data;
return fasync_helper(fd, filep, mode, &md->async_queue);
}
static ssize_t showDevInfo(struct device *dev, struct device_attribute* attr,
char *buf)
{
MyData* md = (MyData*)dev->driver_data;
return snprintf(buf, PAGE_SIZE,
"vendor=0x%x\n"
"device=0x%x\n"
"subsystem_vendor=0x%x\n"
"subsystem_device=0x%x\n"
"revision=0x%x\n"
"bus=0x%x\n"
"slot=0x%x\n"
"func=0x%x\n"
"numBars=%d\n",
md->pdev->vendor,
md->pdev->device,
md->pdev->subsystem_vendor,
md->pdev->subsystem_device,
md->pdev->revision,
md->pdev->bus->number,
PCI_SLOT(md->pdev->devfn),
PCI_FUNC(md->pdev->devfn),
md->numBars);
}
static DEVICE_ATTR(devInfo, 0444, showDevInfo, NULL);
/**
* This structure describe the malibu RTDM device
*
*/
static struct rtdm_device malibu_rtdm_device = {
.struct_version = RTDM_DEVICE_STRUCT_VER,
.device_flags = RTDM_NAMED_DEVICE,
.context_size = sizeof(MyData),
.device_name = DRV_NAME,
.open_rt = malibu_rtdm_open,
.open_nrt = malibu_rtdm_open,
.ops = {
.close_rt = malibu_rtdm_close,
.close_nrt = malibu_rtdm_close,
.ioctl_rt = malibu_rtdm_ioctl,
.ioctl_nrt = malibu_rtdm_ioctl,
//.read_nrt = NULL,
//.write_nrt = NULL,
},
.device_class = RTDM_CLASS_EXPERIMENTAL,
.device_sub_class = SOME_SUB_CLASS,
.profile_version = 1,
.driver_name = DRV_NAME,
.driver_version = RTDM_DRIVER_VER(0, 1, 0),
.peripheral_name = "Malibu RT driver",
.provider_name = "Innovative Integration",
.proc_name = DRV_NAME,
};
static int allocEachDev(struct pci_dev *pdev, const struct pci_device_id *id)
{
struct rtdm_device *rtdm_dev;
int ret_val = 0;
MyData *md, *lPtr;
int i, bCntr, error, b;
MY_PRINT("start");
if(id->vendor != VENDOR_ID) {
MY_ERR("");
return -ENODEV;
}
md = (MyData*) kzalloc(sizeof(MyData), GFP_KERNEL);
md->pdev = pdev;
sprintf(msgbuf, "pdev->irq=%d", pdev->irq);
MY_PRINT(msgbuf);
if (pci_enable_device(pdev)) {
MY_ERR("");
kfree(md);
return -ENODEV;
}
pci_set_master(pdev);
b = 0;
for (i = 0; i < MALIBU_BAR_MAX; i++) {
md->bars[b].start = pci_resource_start(pdev, i);
md->bars[b].end = pci_resource_end(pdev, i);
md->bars[b].size = pci_resource_len(pdev, i);
printk(KERN_ERR "bar=%d start=%llx end=%llx len=%llx\n", i,
md->bars[b].start,
md->bars[b].end,
md->bars[b].size);
if (md->bars[b].size == 0) continue;
md->bars[b].kAddr = (void*) ioremap(md->bars[i].start,
md->bars[i].end - md->bars[i].start);
b++;
}
md->numBars = i;
init_waitqueue_head(&md->intrWait);
cdev_init(&md->cdev, &myFileOps);
md->cdev.owner = THIS_MODULE;
md->myDevT = MKDEV(MAJOR(baseDevT), MINOR(baseDevT)+devCnt);
if (cdev_add(&md->cdev, md->myDevT, 1)) {
MY_ERR("");
pci_disable_device(md->pdev);
kfree(md);
return -ENODEV;
}
bCntr = 0;
list_for_each_entry(lPtr, &devList, list) {
if (lPtr->pdev->device == pdev->device) bCntr++;
}
md->sysdev = device_create(devClass, &md->pdev->dev, md->myDevT, NULL,
MALIBU_DEV_NAME, pdev->device, bCntr);
if (IS_ERR(md->sysdev)) {
MY_ERR("");
cdev_del(&md->cdev);
pci_disable_device(md->pdev);
kfree(md);
return -ENODEV;
}
md->sysdev->driver_data = (void*)md;
error = device_create_file(md->sysdev, &dev_attr_devInfo);
if (error) {
MY_ERR("");
device_destroy(devClass, md->myDevT);
cdev_del(&md->cdev);
for (i = 0; i < MALIBU_BAR_MAX; i++) {
if (md->bars[i].size) {
iounmap(md->bars[i].kAddr);
md->bars[i].size = 0;
}
}
pci_disable_device(md->pdev);
kfree(md);
return error;
}
md->dev_id = devCnt;
//alloc mem for rtdm structure
rtdm_dev = kmalloc(sizeof(struct rtdm_device), GFP_KERNEL);
if(!rtdm_dev){
MY_ERR("rtdm_device kmalloc failed\n");
ret_val = -ENOMEM; //Insufficient storage space is available.
return ret_val;
}
//copy the structure to the new memory
memcpy(rtdm_dev, &malibu_rtdm_device, sizeof(struct rtdm_device));
//create filename
snprintf(rtdm_dev->device_name,
RTDM_MAX_DEVNAME_LEN,
MALIBU_DEV_NAME,
pdev->device, bCntr);
//"malibu_%d", devCnt);
rtdm_dev->device_id = devCnt;
//define two other members of the rtdm_device structure
rtdm_dev->proc_name = rtdm_dev->device_name;
ret_val = rtdm_dev_register(rtdm_dev);
if(ret_val < 0){
MY_ERR("failed to register device with RTDM\n");
ret_val = -ENODEV;
return ret_val;
}
md->xen_rtdm_device = rtdm_dev;
list_add_tail(&md->list, &devList);
//always do this last
devCnt++;
MY_PRINT("end OK");
return 0;
}
static void removeAllDev(struct pci_dev *pdev)
{
MyData *md, *tmp;
MY_PRINT("start");
list_for_each_entry_safe(md, tmp, &devList, list)
{
if (md == NULL)
{
MY_PRINT("md is NULL");
return;
}
int i;
list_del(&md->list);
device_remove_file(md->sysdev, &dev_attr_devInfo);
device_destroy(devClass, md->myDevT);
cdev_del(&md->cdev);
for (i = 0; i < MALIBU_BUF_MAX; i++) freeBuffer(md, i);
for (i = 0; i < MALIBU_BAR_MAX; i++) {
if (md->bars[i].size) {
iounmap(md->bars[i].kAddr);
md->bars[i].size = 0;
}
}
pci_disable_device(md->pdev);
//remove char device from the system
//unregister RTdriver
rtdm_dev_unregister(md->xen_rtdm_device, 1000);
kfree(md->xen_rtdm_device);
kfree(md);
devCnt--;
}
MY_PRINT("end");
}
static struct pci_device_id ids[] = {
{ PCI_DEVICE(VENDOR_ID, PCI_ANY_ID) },
{ 0 },
};
static struct pci_driver pci_driver = {
.name = DRV_NAME,
.id_table = ids,
.probe = allocEachDev,
.remove = removeAllDev,
};
//MODULE_DEVICE_TABLE(pci, ids);
static ssize_t showDevCount(struct class *cls, char *buf)
{
return sprintf(buf, "%d\n", devCnt);
}
static CLASS_ATTR(devCount, 0444, showDevCount, NULL);
/**
* This function is called when the module is loaded
*
* It simply registers the RTDM device.
*
*/
static int __init mod_init(void)
{
int error;
MY_PRINT("initializing MalibuDrv_RT module");
devClass = class_create(THIS_MODULE, DRV_NAME);
if (IS_ERR(devClass)) {
MY_ERR("");
return PTR_ERR(devClass);
}
error = class_create_file(devClass, &class_attr_devCount);
if (error) {
MY_ERR("");
class_destroy(devClass);
return error;
}
error = alloc_chrdev_region(&baseDevT, 0, MALIBU_MAX_DEVS, DRV_NAME);
if (error) {
MY_ERR("");
class_remove_file(devClass, &class_attr_devCount);
class_destroy(devClass);
return error;
}
error = pci_register_driver(&pci_driver);
if (error) {
MY_ERR("");
unregister_chrdev_region(baseDevT, MALIBU_MAX_DEVS);
class_remove_file(devClass, &class_attr_devCount);
class_destroy(devClass);
return error;
}
return error;
}
/**
* This function is called when the module is unloaded
*
* It unregister the RTDM device, polling at 1000 ms for pending users.
*
*/
static void __exit mod_exit(void)
{
MY_PRINT("unregistering MalibuDrv_RT module");
pci_unregister_driver(&pci_driver);
unregister_chrdev_region(baseDevT, MALIBU_MAX_DEVS);
class_remove_file(devClass, &class_attr_devCount);
class_destroy(devClass);
}
module_init(mod_init);
module_exit(mod_exit);
MODULE_LICENSE("GPL");
#ifndef _MALIBU_H_
#define _MALIBU_H_
#include <linux/ioctl.h>
//#include <sys/time.h>
#ifndef __KERNEL__
#include <stdint.h>
#endif
#include <rtdm/rtdm.h>
typedef struct {
uint32_t vendorId;
uint32_t deviceId;
uint32_t subVendorId;
uint32_t subDeviceId;
uint32_t bus;
uint32_t slot;
uint32_t func;
uint8_t revision;
uint8_t numBars;
} MalibuDrv_INFO;
typedef struct {
uint64_t physAddr;
uint64_t fileOffset;
uint32_t size;
uint8_t bufNum;
} MalibuDrv_Buffer;
typedef struct {
uint32_t type;
uint32_t byteOffset;
uint32_t bar;
uint32_t mask;
void (*cntxtFunc)(void*);
void *cntxtArg;
} MalibuDrv_IntCtrl;
#define MALIBU_INTR_MAX 128 // how many isrs can be queued in the
kernel
typedef struct {
struct timeval timeout;
int errno;
void (*cntxtFunc)(void*);
void *cntxtArg;
uint32_t depth;
uint32_t data[MALIBU_INTR_MAX];
} MalibuDrv_DequeueISR;
typedef struct {
uint32_t depth;
uint32_t data[MALIBU_INTR_MAX];
} MalibuDrv_FakeISR;
/**
* IOCTL stuff
*/
struct ioc_param{
uint32_t bv; /* 32bit vector to write to register, */
/* when reading from register, */
/* the value will be stored there */
uint8_t offset; /* offset address of the register we */
/* want to read from or write to */
};
typedef struct {
void *userAddr; /*user mode addr*/
uint64_t size;
uint8_t barnum;
void *kernAddr;
}MalibuDrv_MmapArg;
typedef struct ioc_param ioc_param_struct;
#define MALIBU_IOC_MAGIC 'm'
#define MALIBU_GET_INFO _IOR(MALIBU_IOC_MAGIC, 0, MalibuDrv_INFO)
#define MALIBU_GET_BAR_SIZE _IO(MALIBU_IOC_MAGIC, 1)
#define MALIBU_ALLOC_BUFFER _IOWR(MALIBU_IOC_MAGIC, 2, MalibuDrv_Buffer)
#define MALIBU_FREE_BUFFER _IOW(MALIBU_IOC_MAGIC, 3, MalibuDrv_Buffer)
#define MALIBU_ISR_ENABLE _IO(MALIBU_IOC_MAGIC, 4)
#define MALIBU_ISR_DISABLE _IO(MALIBU_IOC_MAGIC, 5)
#define MALIBU_GET_ISR_ENABLED _IO(MALIBU_IOC_MAGIC, 6)
#define MALIBU_GET_ISR_DEPTH _IO(MALIBU_IOC_MAGIC, 7)
#define MALIBU_ISR_CLEAR_ALL _IO(MALIBU_IOC_MAGIC, 8)
#define MALIBU_DEQUEUE_ISR _IOWR(MALIBU_IOC_MAGIC, 9, MalibuDrv_DequeueISR)
#define MALIBU_SET_ISR_CTRL _IOW(MALIBU_IOC_MAGIC, 10, MalibuDrv_IntCtrl)
#define MALIBU_SET_SIG _IO(MALIBU_IOC_MAGIC, 11)
#define MALIBU_FAKE_SIG _IOW(MALIBU_IOC_MAGIC, 12, MalibuDrv_FakeISR)
#define MALIBU_MMAP_BAR _IOWR(MALIBU_IOC_MAGIC, 13, MalibuDrv_MmapArg)
#define MALIBU_MMAP_BLOCK _IOWR(MALIBU_IOC_MAGIC, 14, MalibuDrv_MmapArg)
#define MALIBU_MAX_DEVS 256
#define MALIBU_MMAP_SIZE (16 * 1024 * 1024)
#define MALIBU_BAR_MAX 6
#define MALIBU_BUF_MAX 2
#define MALIBU_DEV_NAME "malibu%04x_%d"
#define MALIBU_BUF_MAP_NR 9
#define MALIBU_BUF_MAP (1 << MALIBU_BUF_MAP_NR)
#endif
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#define __USE_GNU
#include <fcntl.h>
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
#include "malibu.h"
struct timeval startTime;
void hello_world(char* data)
{
printf("hello frm hello_world\n");
printf("data=%x", data);
}
int checkFd(int fd, int secs)
{
int ret;
MalibuDrv_DequeueISR isr;
struct timeval now;
double diff;
int i;
isr.timeout.tv_sec = secs;
isr.timeout.tv_usec = 0;
if (rt_dev_ioctl(fd, MALIBU_DEQUEUE_ISR, &isr) < 0) {
perror("rt_dev_ioctl");
}
else {
gettimeofday(&now, 0);
diff = now.tv_sec - startTime.tv_sec + (now.tv_usec -
startTime.tv_usec) / 1e6;
if (isr.errno) {
char buf[256];
strerror_r(isr.errno, buf, sizeof(buf));
printf("%15.f: fd=%d error=%s\n", diff, fd, buf);
}
else {
printf("%15.6f: fd=%d got depth=%d",
diff,
fd,
isr.depth);
if (secs != 0) printf(" time remaining=%f",
isr.timeout.tv_sec + isr.timeout.tv_usec / 1e6);
printf("\n");
for (i = 0; i < isr.depth; i++) {
printf("\tdata[%d] = %08x\n", i, isr.data[i]);
}
if (isr.cntxtFunc) (*isr.cntxtFunc)(isr.cntxtArg);
}
}
return ret;
}
int doExit = 0;
void* backgroundThr(void* arg)
{
while (1) {
if (checkFd((long)arg, 2) < 0) break;
if (doExit) break;
}
printf("thread fd=%d exiting\n", (long) arg);
}
void myHandler(int sig, siginfo_t* info, void* arg)
{
int i;
if (sig == SIGPOLL && info->si_code == POLL_IN) {
checkFd(info->si_fd, 0);
}
}
int main(int argc, char * argv[])
{
struct sigaction act[8];
pthread_t tid[8];
int numBoards = 0;
int fds[8];
int i, j, boardCnt;
char *bars[8][MALIBU_BAR_MAX];
int barSize[8][MALIBU_BAR_MAX];
int barCnt[8];
char *bufPtrs[8][MALIBU_BUF_MAX];
MalibuDrv_Buffer bufs[8][MALIBU_BUF_MAX];
for (i = 0; i < 8; i++) {
struct stat sbuf;
char buf[128];
sprintf(buf, "/dev/malibu%04x_%d", 0x30, i);
if (stat(buf, &sbuf) < 0) {
break;
}
else {
printf("file \"%s\" exists\n", buf);
}
}
numBoards = i;
printf("numBoards = %d\n", numBoards);
for (i = 0; i < numBoards; i++)
{
char buf[128];
MalibuDrv_INFO info;
//sprintf(buf, "malibu_%d", i);
sprintf(buf, MALIBU_DEV_NAME, 0x30, i);
fds[i] = rt_dev_open(buf, O_RDWR);
if (fds[i] < 0)
{
sprintf(buf, "%s open failed", buf,i);
perror(buf);
return -1;
} else
printf("%s open OK, fds[%d]=%d\n", buf, i, fds[i]);
printf("calling rt_dev_ioctl::MALIBU_GET_INFO\n");
if (rt_dev_ioctl(fds[i], MALIBU_GET_INFO, &info) < 0)
{
perror("ioctl MALIBU_GET_INFO");
exit(-1);
}
printf("%d: vendorId=%x, deviceId=%x, subVendorId=%x,
subDeviceId=%x, bus=%x, slot=%x, func=%x revision=%d numBars=%d\n",
i, info.vendorId, info.deviceId,
info.subVendorId, info.subDeviceId,
info.bus, info.slot, info.func,
info.revision,
info.numBars);
bufs[i][0].size = 4 * 1024 * 1024;
bufs[i][0].bufNum = 0;
if (rt_dev_ioctl(fds[i], MALIBU_ALLOC_BUFFER, &bufs[i][0]) < 0)
{
perror("rt_dev_ioctl MALIBU_ALLOC_BUFFER");
exit(-1);
}
printf("MALIBU_ALLOC_BUFFER: bufs.physAddr=%p\n",
bufs[i][0].physAddr);
bufs[i][1].size = 1024 * 1024;
bufs[i][1].bufNum = 1;
if (rt_dev_ioctl(fds[i], MALIBU_ALLOC_BUFFER, &bufs[i][1]) < 0)
{
perror("rt_dev_ioctl MALIBU_ALLOC_BUFFER");
exit(-1);
}
printf("MALIBU_ALLOC_BUFFER: bufs.physAddr=%p\n",
bufs[i][1].physAddr);
printf("calling rt_dev_ioctl::MALIBU_GET_BAR_SIZE\n");
int bar=0;
int barSize = rt_dev_ioctl(fds[i], MALIBU_GET_BAR_SIZE, &bar);
printf("bar=%d, barSize = %d", bar, barSize);
printf("calling rt_dev_ioctl::MALIBU_FREE_BUFFER\n");
rt_dev_ioctl(fds[i], MALIBU_GET_BAR_SIZE, &bufs[i][0]);
rt_dev_ioctl(fds[i], MALIBU_GET_BAR_SIZE, &bufs[i][1]);
#if 0
for (barCnt[i] = 0; barCnt[i] < MALIBU_BAR_MAX; barCnt[i]++) {
barSize[i][barCnt[i]] = rt_dev_ioctl(fds[i],
MALIBU_GET_BAR_SIZE, barCnt[i]);
if (barSize[i][barCnt[i]] < 0) {
perror("rt_dev_ioctl MALIBU_BAR_GET_SIZE");
exit(-1);
}
if (barSize[i][barCnt[i]] == 0) break;
printf("\tbarSize[%d][%d]=0x%x\n", i, barCnt[i],
barSize[i][barCnt[i]]);
}
for (j = 0; j < barCnt[i]; j++) {
bars[i][j] = (char*)mmap(0, barSize[i][j],
PROT_READ|PROT_WRITE, MAP_SHARED, fds[i], j * MALIBU_MMAP_SIZE);
if ((void*)bars[i][j] == MAP_FAILED) {
perror("mmap");
exit(-1);
}
printf("\tbar[%d][%d] = %p\n", i, j, bars[i][j]);
}
for (j = 0; j < 2; j++) {
bufPtrs[i][j] = (char*)mmap(0, bufs[i][j].size,
PROT_READ|PROT_WRITE, MAP_SHARED, fds[i], (j + MALIBU_BAR_MAX) *
MALIBU_MMAP_SIZE);
if ((void*)bufPtrs[i][j] == MAP_FAILED) {
perror("mmap");
exit(-1);
}
printf("\tbufPtrs[%d][%d] = %p, physAddr=%llx\n", i, j,
bufPtrs[i][j], bufs[i][j].physAddr);
}
#endif
}
#if 0
boardCnt = i;
for (i = 0; i < boardCnt; i++) {
for (j = 0; j < boardCnt; j++) {
printf("board %d buf %d = %08x %08x %08x\n", i, j,
((uint32_t*)bufPtrs[i][j])[0],
((uint32_t*)bufPtrs[i][j])[1],
((uint32_t*)bufPtrs[i][j])[2]);
}
}
gettimeofday(&startTime, 0);
for (i = 0; i < boardCnt; i++) {
pthread_create(&tid[i], NULL, backgroundThr, (void*)fds[i]);
}
printf("calling rt_dev_ioctl::MALIBU_FAKE_SIG\n");
for (i = 0; i < 10; i++) {
int j;
MalibuDrv_FakeISR x;
sleep(1);
x.depth = i + 1;
for (j = 0; j < x.depth; j++) x.data[j] = 16 * i + j;
if (i & 1) rt_dev_ioctl(fds[0], MALIBU_FAKE_SIG, &x);
else rt_dev_ioctl(fds[1], MALIBU_FAKE_SIG, &x);
}
doExit = 1;
sleep(4);
printf("threads killed\n");
printf("calling rt_dev_ioctl::MALIBU_SET_ISR_CTRL\n");
for (i = 0; i < numBoards; i++) {
MalibuDrv_IntCtrl isrCtrl;
isrCtrl.bar = 0;
isrCtrl.byteOffset = 0;
isrCtrl.cntxtFunc = (void (*)(void*)) hello_world;
isrCtrl.cntxtArg = (void*) malloc(128);
sprintf((char*)isrCtrl.cntxtArg, "hello from %d\n", i);
if (rt_dev_ioctl(fds[i], MALIBU_SET_ISR_CTRL, &isrCtrl) < 0) {
perror("new rt_dev_ioctl\n");
exit(-1);
}
act[i].sa_sigaction = myHandler;
act[i].sa_flags = SA_SIGINFO;
sigemptyset(&act[i].sa_mask);
sigaction(SIGIO, act+i, 0);
// rt_dev_ioctl(fds[i], MALIBU_SET_SIG, SIGIO);
fcntl(fds[i], F_SETFL, O_ASYNC);
fcntl(fds[i], F_SETOWN, getpid());
fcntl(fds[i], F_SETSIG, SIGIO);
}
printf("MALIBU_FAKE_SIG\n");
for (i = 0; i < 10; i++) {
int j;
MalibuDrv_FakeISR x;
sleep(1);
x.depth = i + 1;
for (j = 0; j < x.depth; j++) x.data[j] = 16 * i + j;
if (i & 1) rt_dev_ioctl(fds[0], MALIBU_FAKE_SIG, &x);
else rt_dev_ioctl(fds[1], MALIBU_FAKE_SIG, &x);
}
for (i = 0; i < boardCnt; i++) {
for (j = 0; j < barCnt[i]; j++) {
if (munmap(bars[i][j], barSize[i][j]) < 0) {
perror("munmap");
exit(-1);
}
}
for (j = 0; j < 2; j++) {
if (munmap(bufPtrs[i][j], bufs[i][j].size) < 0) {
perror("munmap");
exit(-1);
}
}
}
#endif
sleep(3);
exit(0);
}
_______________________________________________
Xenomai-help mailing list
[email protected]
https://mail.gna.org/listinfo/xenomai-help
