//
static int bad_address(void *p)
{
unsigned long dummy;
return probe_kernel_address((unsigned long*)p, dummy);
}
/*
* map any virtual address of the current process to its
* physical one.
*/
static unsigned long long any_v2p(unsigned long long vaddr)
{
pgd_t *pgd = pgd_offset(current->mm, vaddr);
p4d_t *p4d;
pud_t *pud;
pmd_t *pmd;
pte_t *pte;
/* to lock the page */
struct page *pg;
unsigned long long paddr;
if (bad_address(pgd)) {
printk(KERN_ALERT "[nskk] Alert: bad address of pgd %p\n", pgd);
goto bad;
}
if (!pgd_present(*pgd)) {
printk(KERN_ALERT "[nskk] Alert: pgd not present %lu\n", *pgd);
goto out;
}
p4d = p4d_offset(pgd, vaddr);
if (p4d_none(*p4d))
return 0;
pud = pud_offset(p4d, vaddr);
pud = pud_offset(pgd, vaddr);
if (bad_address(pud)) {
printk(KERN_ALERT "[nskk] Alert: bad address of pud %p\n", pud);
goto bad;
}
if (!pud_present(*pud) || pud_large(*pud)) {
printk(KERN_ALERT "[nskk] Alert: pud not present %lu\n", *pud);
goto out;
}
pmd = pmd_offset(pud, vaddr);
if (bad_address(pmd)) {
printk(KERN_ALERT "[nskk] Alert: bad address of pmd %p\n", pmd);
goto bad;
}
if (!pmd_present(*pmd) || pmd_large(*pmd)) {
printk(KERN_ALERT "[nskk] Alert: pmd not present %lu\n", *pmd);
goto out;
}
pte = pte_offset_kernel(pmd, vaddr);
if (bad_address(pte)) {
printk(KERN_ALERT "[nskk] Alert: bad address of pte %p\n", pte);
goto bad;
}
if (!pte_present(*pte)) {
printk(KERN_ALERT "[nskk] Alert: pte not present %lu\n", *pte);
goto out;
}
pg = pte_page(*pte);
paddr = (pte_val(*pte) & PHYSICAL_PAGE_MASK) | (vaddr&(PAGE_SIZE-1));
pte->pte |= _PAGE_RW; // | _PAGE_USER;
paddr = pte_val(*pte);
out:
return paddr;
bad:
printk(KERN_ALERT "[nskk] Alert: Bad address\n");
return 0;
}
static int virt2phys_open(struct inode *inode, struct file *filp)
{
printk("%s\n", __func__);
return 0;
}
static ssize_t virt2phys_read(struct file *filp, char __user *buf,
size_t count, loff_t *ppos)
{
int copy_len;
printk("%s\n", __func__);
copy_len = 0;
return copy_len;
}
static ssize_t virt2phys_write(struct file *filp, const char __user *buf,
size_t count, loff_t *ppos)
{
int copy_len;
copy_len = 0;
return copy_len;
}
static int virt2phys_release(struct inode *inode, struct file *filp)
{
printk("%s\n", __func__);
return 0;
}
static unsigned int virt2phys_poll( struct file* filp, poll_table* wait )
{
unsigned int retmask = 0;
printk("%s\n", __func__);
retmask |= POLLHUP;
/*
Set POLLHUP in retmask if read device is EOF
Set POLLERR if the device is in error
Set POLLPRI if out-of-band data can be read
*/
return retmask;
}
static long virt2phys_ioctl(struct file *filp,
unsigned int cmd, unsigned long arg)
{
unsigned long long *ptr, ret;
printk("%s\n", __func__);
if (cmd == 1) {
ptr = (unsigned long long *)arg;
printk( "VA=%p\n", (unsigned long long *)*ptr);
ret = any_v2p(*ptr);
printk( "PA=%p\n", (unsigned long long *)ret);
*ptr = ret;
return 0;
}
return -ENOTTY;
}
static struct file_operations virt2phys_fops = {
.owner = THIS_MODULE,
.read = virt2phys_read,
.write = virt2phys_write,
.poll = virt2phys_poll,
.unlocked_ioctl = virt2phys_ioctl,
.open = virt2phys_open,
.release = virt2phys_release,
};
static struct miscdevice virt2phys_dev = {
.minor = MISC_DYNAMIC_MINOR,
.name = DRV_NAME,
.fops = &virt2phys_fops,
};
static int __init virt2phys_init(void)
{
int ret;
pr_info(virt2phys_DRIVER_NAME "\n");
printk("%s\n", __func__);
ret = misc_register(&virt2phys_dev);
if (ret) {
printk("fail to misc_register (MISC_DYNAMIC_MINOR)\n");
goto error;
}
return 0;
error:
return ret;
}
static void __exit virt2phys_cleanup(void)
{
misc_deregister(&virt2phys_dev);
printk("%s\n", __func__);
}
MODULE_LICENSE("GPL");
module_init(virt2phys_init);
module_exit(virt2phys_cleanup);
