Add protocolHEADmaster

1 files changed, 160 insertions, 0 deletions

diff --git a/sad_mod.c b/sad_mod.c
new file mode 100644
index 0000000..2950b35
--- /dev/null
+++ b/sad_mod.c
@@ -0,0 +1,160 @@
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/cdev.h>
+#include <linux/semaphore.h>
+
+MODULE_LICENSE("GPL");
+
+#define DEV_NAME "sad_dev"
+
+static int sad_open(struct inode *, struct file *);
+static int sad_release(struct inode *, struct file *);
+static ssize_t sad_read(struct file *, char __user *, size_t, loff_t *);
+static ssize_t sad_write(struct file *, const char __user *, size_t, loff_t *);
+
+static dev_t dev;
+static struct cdev *cdev;
+static int device_open_count = 0;
+
+#define LL_MAX_LEN 20
+
+struct sad_buf {
+    char buf[LL_MAX_LEN + 1];
+    size_t size;
+    struct semaphore lock;
+} *buf;
+
+static struct file_operations sad_fops = {
+    .owner = THIS_MODULE,
+    .read = sad_read,
+    .write = sad_write,
+    .open = sad_open,
+    .release = sad_release
+};
+
+static ssize_t sad_parse(const char __user *user_buf, size_t len) {
+    char *in_buf;
+    unsigned long uncopied;
+    long long lhs, rhs, res;
+    char op;
+    int ret;
+
+    in_buf = kmalloc(len, GFP_KERNEL);
+    if (!in_buf)
+        return -ENOMEM;
+
+    uncopied = copy_from_user(in_buf, user_buf, len);
+    if (uncopied > 0) {
+        kfree(in_buf);
+        return -EFAULT;
+    }
+
+    sscanf(in_buf, "%lld%c%lld", &lhs, &op, &rhs);
+
+    switch (op) {
+        case '+': res = lhs + rhs; break;
+        case '-': res = lhs - rhs; break;
+        case '*': res = lhs * rhs; break;
+        case '/': res = lhs / rhs; break;
+        case '%': res = lhs & rhs; break;
+        default:
+            kfree(in_buf);
+            return -EPROTO;
+    }
+
+    ret = snprintf(buf->buf, LL_MAX_LEN + 1, "%lld", res);
+    buf->size = ret;
+
+    return len; /* need to indicate, that all data was processed */
+}
+
+static ssize_t sad_read(struct file *flip, char __user *user_buf, size_t len, loff_t *offset) {
+    size_t bytes_left, read_size, uncopied;
+
+    if (down_interruptible(&buf->lock))
+        return -ERESTARTSYS;
+
+    if (*offset >= buf->size) {
+        up(&buf->lock);
+        return 0;
+    }
+
+    bytes_left = buf->size - *offset;
+    read_size = min(bytes_left, len);
+    uncopied = copy_to_user(user_buf, buf->buf + *offset, read_size);
+
+    *offset += read_size - uncopied;
+
+    up(&buf->lock);
+    return read_size - uncopied;
+}
+
+static ssize_t sad_write(struct file *flip, const char __user *user_buf, size_t len, loff_t *offset) {
+    ssize_t ret;
+
+    if (down_interruptible(&buf->lock))
+        return -ERESTARTSYS;
+
+    ret = sad_parse(user_buf, len);
+    up(&buf->lock);
+    return ret;
+}
+
+static int sad_open(struct inode *inode, struct file *file) {
+    device_open_count++;
+    try_module_get(THIS_MODULE);
+    return 0;
+}
+
+static int sad_release(struct inode *inode, struct file *file) {
+    device_open_count--;
+    if (device_open_count == 0)
+        module_put(THIS_MODULE);
+    return 0;
+}
+
+static int __init sad_init(void) {
+    int rc;
+
+    const unsigned int minor = 0;
+    const unsigned int count = 1;
+
+    buf = kzalloc(sizeof(*buf), GFP_KERNEL);
+    if (!buf)
+        return -ENOMEM;
+
+    sema_init(&buf->lock, 1);
+
+    rc = alloc_chrdev_region(&dev, minor, count, DEV_NAME);
+    if (rc < 0) {
+        printk(KERN_ALERT "Could not register device: %d\n", rc);
+        return rc;
+    } else {
+        printk(KERN_INFO "Allocated maj %d\n", MAJOR(dev));
+    }
+
+    cdev = cdev_alloc();
+    cdev->owner = THIS_MODULE;
+    cdev->ops = &sad_fops;
+    rc = cdev_add(cdev, dev, 1);
+
+    if (rc) {
+        printk(KERN_ALERT "Could not cdev_add: %d\n", rc);
+        return rc;
+    }
+
+    return 0;
+}
+static void __exit sad_exit(void) {
+    cdev_del(cdev);
+    kfree(buf);
+    unregister_chrdev_region(dev, 1);
+    printk(KERN_INFO "okay, bye\n");
+}
+
+module_init(sad_init);
+module_exit(sad_exit);