arm_mrc.c

#include <uapi/linux/fs.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/uaccess.h>
#include <linux/slab.h>
#include <linux/vmalloc.h>
#include <linux/init.h>
#include <linux/cdev.h>
#include <linux/fs.h>
#include <asm/cacheflush.h>

MODULE_LICENSE("Dual BSD/GPL");
MODULE_AUTHOR("ikiuo");
MODULE_DESCRIPTION("Invoke MRC instruction");
MODULE_VERSION("0.1.0");

#if defined(DEBUG)
#define debug(x)  printk x
#else
#define debug(x)
#endif

typedef struct class class_type;
typedef struct device device_type;
typedef struct cdev chrdev_type;
typedef struct inode inode_type;
typedef struct file file_type;
typedef struct file_operations file_handler;

typedef unsigned int (*mrc_func)(void);

static void *mrc_proc = NULL;
#define INS_MRC       0xee100010
#define INS_MOV_PC_LR 0xe1a0f00e /* MOV PC,LR */
#define INS_BX_LR     0xe12fff1e /* BX  LR    */

/* addr = [1:mode][4:cop][3:op1][4:CRn][4:CRm][3:op2][2:00] */
static const loff_t offset_max = (1 << (1+4+3+4+4+3+2));

static loff_t
device_llseek (file_type *file, loff_t offset, int mode)
{
  debug((KERN_INFO "lseek(%p, %Ld, %d): call\n", file, offset, mode));

  switch (mode)
    {
    default: return -EINVAL;
    case SEEK_SET: offset += file->f_pos; break;
    case SEEK_CUR: break;
    case SEEK_END: offset += offset_max; break;
    }
  if (offset < 0) return -EINVAL;
  if (offset > offset_max) return -EINVAL;
  file->f_pos = offset;

  debug((KERN_INFO "lseek(%p, %Ld, %d): end\n", file, offset, mode));

  return offset;
}

static ssize_t
device_read (file_type *file, char *buffer, size_t size, loff_t *offset)
{
  loff_t addr, rem, pos = *offset;
  ssize_t total = 0;
  size_t res_size;
  loff_t res_pos;

  unsigned int *ins = (unsigned int *) mrc_proc;
  unsigned int cop, CRn, op1, CRm, op2;
  loff_t mode;
  unsigned int status;

  if (pos < 0) return -EIO;
  if (pos >= offset_max) return -EIO;

  rem = offset_max - pos;
  if (size > rem) size = rem;

  while (size > 0)
    {
      addr = pos >> 2;

      op2 = (addr >>  0) &  7;
      CRm = (addr >>  3) & 15;
      CRn = (addr >>  7) & 15;
      op1 = (addr >> 11) &  7;
      cop = (addr >> 14) & 15;
      mode = (addr >> 18);

      debug((KERN_INFO "read(%p, %p, %d, %Ld)\n", file, buffer, size, *offset));

      status = 0;
      if (mode != 1)
          return -EIO;

      ins[0] = INS_MRC | (op1 << 21) | (CRn << 16) | (cop << 8) | (op2 << 5) | CRm;
      ins[1] = INS_MOV_PC_LR;
      flush_kernel_vmap_range (ins, 16);
      flush_icache_range ((unsigned long)ins,16);
      status = ((mrc_func)ins) ();

      res_pos = pos & 3;
      res_size = 4 - res_pos;
      if (res_size > size)
        res_size = size;

      __copy_to_user (buffer, ((char*)&status) + res_pos, res_size);

      buffer += res_size;
      size   -= res_size;
      pos    += res_size;
      total  += res_size;
    }
  *offset = pos;
  return total;
}

static int
device_open (inode_type *inode, file_type *file)
{
  return (try_module_get (THIS_MODULE) ? 0 : -EBUSY);
}

static int
device_release (inode_type *inode, file_type *file)
{
  module_put (THIS_MODULE);
  return 0;
}

static file_handler handler = {
  .llseek  = device_llseek,
  .read    = device_read,
  .open    = device_open,
  .release = device_release,
};
static chrdev_type cdev_mrc;

#define DEVICE_NAME "arm_mrc"
static const int arm_ins_buffer_type = 1;
static const char *device_name = DEVICE_NAME;
static int major_number = 0;
static int minor_base = 0;
static int minor_count = 1;
static class_type *device_class = NULL;
static device_type *sys_device = NULL;

static void *
arm_alloc_ins_buffer (void)
{
  if (arm_ins_buffer_type)
    return __vmalloc (256, GFP_KERNEL, PAGE_KERNEL_EXEC);
  else
    return kmalloc (256, GFP_KERNEL);
}

static void
arm_free_ins_buffer (void *p)
{
  if (p == NULL)
    return;
  if (arm_ins_buffer_type)
    vfree (p);
  else
    kfree (p);
}

static void
arm_mrc_exit_chrdev (void)
{
  dev_t dev = MKDEV (major_number, minor_base);
  unregister_chrdev_region (dev, minor_count);
  arm_free_ins_buffer (mrc_proc);
}

static void
arm_mrc_exit_cdev_chrdev (void)
{
  cdev_del (&cdev_mrc);
  arm_mrc_exit_chrdev ();
}

static void
arm_mrc_exit_all (void)
{
  device_destroy (device_class, MKDEV (major_number, minor_base));
  class_destroy (device_class);
  arm_mrc_exit_cdev_chrdev ();
}

static int
__init arm_mrc_init (void)
{
  dev_t dev;
  int stat;

  mrc_proc = arm_alloc_ins_buffer ();
  if (mrc_proc == NULL) return -ENOMEM;

  stat = alloc_chrdev_region (&dev, minor_base, minor_count, device_name);
  if (stat < 0)
    {
      printk (KERN_ERR DEVICE_NAME ": alloc_chrdev_region failed: %d\n", stat);
      return stat;
    }

  major_number = MAJOR (dev);
  cdev_init (&cdev_mrc, &handler);
  cdev_mrc.owner = THIS_MODULE;

  stat = cdev_add (&cdev_mrc, dev, minor_count);
  if (stat < 0)
    {
      printk (KERN_ERR DEVICE_NAME ": cdev_add failed: %d\n", stat);
      arm_mrc_exit_chrdev ();
      return stat;
    }

  device_class = class_create (THIS_MODULE, device_name);
  if (IS_ERR(device_class))
    {
      printk (KERN_ERR DEVICE_NAME ": class_create failed\n");
      arm_mrc_exit_cdev_chrdev ();
      return -1;
    }

  sys_device = device_create(device_class, NULL, dev, NULL, DEVICE_NAME);
  if (IS_ERR(sys_device))
    {
      arm_mrc_exit_all ();
      return -1;
    }

  return 0;
}

static void
__exit arm_mrc_exit (void)
{
  arm_mrc_exit_all ();
}

module_init (arm_mrc_init);
module_exit (arm_mrc_exit);

Makefile

# Makefile

#MOD_DIR = /lib/modules/4.9.0-6-rpi
#MOD_DIR = /lib/modules/4.19.93+
#MOD_DIR = /lib/modules/4.19.93-v7+
MOD_DIR = /lib/modules/4.19.93-v7l+

DEVNAME = arm_mrc
KMOD    = $(DEVNAME).ko

obj-m += $(DEVNAME).o

all:	build

build:	$(KMOD)

$(KMOD): $(DEVNAME).c
	make -C $(MOD_DIR)/build M=$(PWD) modules

clean:
	make -C $(MOD_DIR)/build M=$(PWD) clean

insmod: build
	sudo cp 99-arm_mrc.rules /etc/udev/rules.d/
	sudo insmod $(KMOD)

rmmod:
	sudo rmmod $(KMOD)

99-arm_mrc.rules

KERNEL=="arm_mrc", GROUP="root", MODE="0666"

test.c

#include <stdio.h>
#include <stdlib.h>

#include <errno.h>
#include <fcntl.h>
#include <unistd.h>

int
main (int argc, char **argv)
{
  int debug = 0;
  int coproc = 0;
  int opcode1 = 0;
  int CRn = 0;
  int CRm = 0;
  int opcode2 = 0;

  int fd;
  off_t addr, spos;
  ssize_t rdsz;

  unsigned int value;
  int exitcode = 0;

  if (argc < 2)
    {
      printf ("Usage: %s coproc [opcode1 [CRn [CRm [opcode2]]]]\n", argv[0]);
      return 1;
    }

  if (argc > 1) coproc  = strtol (argv[1], NULL, 10);
  if (argc > 2) opcode1 = strtol (argv[2], NULL, 10);
  if (argc > 3) CRn     = strtol (argv[3], NULL, 10);
  if (argc > 4) CRm     = strtol (argv[4], NULL, 10);
  if (argc > 5) opcode2 = strtol (argv[5], NULL, 10);

  if ((coproc > 15) || (opcode1 > 7) || (CRn > 15)
      || (CRm > 15) || (opcode2 > 7))
    {
      printf ("invalid argument.\n");
      printf ("coproc=%d, opcode1=%d, CRn=%d, CRm=%d, opcode2=%d\n",
              coproc, opcode1, CRn, CRm, opcode2);
      return 1;
    }
  addr = ((1 << 20) | (coproc << 16) | (opcode1 << 13)
          | (CRn << 9) | (CRm << 5) | (opcode2 << 2));

  fd = open ("/dev/arm_mrc", O_RDONLY);
  if (fd < 0)
    {
      printf ("Can't open '/dev/arm_mrc': error=%d (%d)\n", fd, errno);
      return 2;
    }

  spos = lseek (fd, addr, SEEK_SET);
  if (addr != spos)
    {
      printf ("seek error: error=%d (%d)\n", spos, errno);
      exitcode = 2;
    }
  else
    {
      rdsz = read (fd, &value, sizeof (value));
      if (rdsz != sizeof (value))
        {
          printf ("read error: error=%d (%d)\n", spos, errno);
          exitcode = 2;
        }
      else
        {
          printf ("MRC p%d,#%d,Rt,c%d,c%d,#%d; Rt=%#010x\n",
                  coproc, opcode1, CRn, CRm, opcode2, value);
        }
    }

  close (fd);
  return 0;
}