kmod.c

#include "../common/include/measurement.h"
#include <linux/cdev.h>
#include <linux/crc32.h>
#include <linux/delay.h>
#include <linux/fs.h>
#include <linux/gpio.h>
#include <linux/hrtimer.h>
#include <linux/kernel.h>
#include <linux/kthread.h>
#include <linux/module.h>
#include <linux/mutex.h>
#include <linux/uaccess.h>
#include <linux/vmalloc.h>

#define DEVICE_NAME "sibyl"
#define CLASS_NAME "sibyl_class"

#define SLOTS 4
#define SLOT_INTERVAL_NS 100000000

#define MAX_MEASUREMENTS 16

#define BAUD_RATE 9600
#define BIT_TIME_US (1000000 / BAUD_RATE)
#define GPIO_PIN 575

static int major_number;
static struct class *class = NULL;
static struct cdev mycdev;

typedef struct packet {
  uint8_t magic;
  data_t data;
  uint32_t crc;
} packet_t;

static packet_t *packet;

static struct hrtimer timer;
static ktime_t interval;

static struct task_struct *timed_thread;

static DECLARE_WAIT_QUEUE_HEAD(wq);
static atomic_t wake_counter = ATOMIC_INIT(0);

// Send Buffers
#define SEND_BUFFER_SIZE 4096

static struct mutex send_buffer_mutex;

static uint8_t *primary_send_buffer;
static size_t primary_send_buffer_size = 0;

static uint8_t *secondary_send_buffer;
static size_t secondary_send_buffer_size = 0;

static int slot = 0;

static int device_open(struct inode *inode, struct file *file) { return 0; }

static int device_release(struct inode *inode, struct file *file) { return 0; }

static uint32_t calculate_crc32(const packet_t *packet) {
  uint32_t crc = crc32(0, (void *)&packet->magic, sizeof(uint8_t));
  crc = crc32(crc, (void *)&packet->data.sender_id, sizeof(uint8_t));
  crc = crc32(crc, (void *)&packet->data.count, sizeof(uint8_t));
  crc = crc32(crc, (void *)&packet->data.measurements,
              sizeof(measurement_t) * packet->data.count);
  return crc;
}

static void prepare_for_sending(void) {
  packet->crc = calculate_crc32(packet);

  size_t n = 0;

  secondary_send_buffer[0] = packet->magic;
  secondary_send_buffer[1] = packet->data.sender_id;
  secondary_send_buffer[2] = packet->data.count;

  n += 3;

  for (size_t a = 0; a < packet->data.count; a++) {
    uint8_t *bytes = (uint8_t *)&(packet->data.measurements[a]);

    // 10 bytes per measurement
    for (size_t b = 0; b < 10; b++) {
      secondary_send_buffer[n + b] = bytes[b];
    }

    n += 10;
  }

  uint8_t *bytes = (uint8_t *)&packet->crc;

  for (size_t i = 0; i < sizeof(uint32_t); i++) {
    secondary_send_buffer[n + i] = bytes[i];
  }

  n += sizeof(uint32_t);

  secondary_send_buffer_size = n;

  // Swap buffers
  mutex_lock(&send_buffer_mutex);

  uint8_t *tmp = primary_send_buffer;
  primary_send_buffer = secondary_send_buffer;
  secondary_send_buffer = tmp;

  size_t tmp_size = primary_send_buffer_size;
  primary_send_buffer_size = secondary_send_buffer_size;
  secondary_send_buffer_size = tmp_size;

  mutex_unlock(&send_buffer_mutex);
}

static ssize_t device_write(struct file *filp, const char *input, size_t length,
                            loff_t *offset) {
  data_t temp_data;

  if (copy_from_user(&temp_data, input, sizeof(data_t))) {
    return -EFAULT;
  }

  if (temp_data.count >= MAX_MEASUREMENTS) {
    printk(KERN_ALERT "lkm: Tried to write more measurements than allowed\n");
    return -EFAULT;
  }

  packet->data.sender_id = temp_data.sender_id;
  packet->data.count = temp_data.count;

  if (temp_data.measurements != NULL) {
    if (copy_from_user(packet->data.measurements, temp_data.measurements,
                       sizeof(measurement_t) * temp_data.count)) {
      return -EFAULT;
    }
  }

  prepare_for_sending();

  return length;
}

static const struct file_operations fops = {
    .write = device_write, .open = device_open, .release = device_release};

static enum hrtimer_restart timer_callback(struct hrtimer *timer) {
  atomic_inc(&wake_counter);
  wake_up_interruptible(&wq);

  hrtimer_forward_now(timer, interval);
  return HRTIMER_RESTART;
}

static inline void send_bit(uint8_t bit) {
  gpio_set_value(GPIO_PIN, bit);
  udelay(BIT_TIME_US);
}

static inline void send_byte(uint8_t byte) {
  send_bit(0);

  send_bit((byte >> 0) & 1);
  send_bit((byte >> 1) & 1);
  send_bit((byte >> 2) & 1);
  send_bit((byte >> 3) & 1);
  send_bit((byte >> 4) & 1);
  send_bit((byte >> 5) & 1);
  send_bit((byte >> 6) & 1);
  send_bit((byte >> 7) & 1);

  send_bit(1);
}

static void send_buffer(void) {
  printk(KERN_INFO "Send %zu bytes\n", primary_send_buffer_size);

  unsigned long flags;
  local_irq_save(flags);

  for (size_t i = 0; i < primary_send_buffer_size; i++) {
    send_byte(primary_send_buffer[i]);
  }

  local_irq_restore(flags);
}

static int timed_thread_fn(void *args) {
  while (!kthread_should_stop()) {
    wait_event_interruptible(wq, atomic_read(&wake_counter) > 0);
    atomic_dec(&wake_counter);

    if (slot == 0) {
      mutex_lock(&send_buffer_mutex);
      send_buffer();
      mutex_unlock(&send_buffer_mutex);
    }

    slot = (slot + 1) % SLOTS;
  }

  return 0;
}

static int __init lkm_init(void) {
  dev_t dev;

  mutex_init(&send_buffer_mutex);

  // Allocate memory for packet
  packet = (packet_t *)vmalloc(sizeof(packet_t));

  if (packet == NULL) {
    return -EFAULT;
  }

  packet->magic = 0xAA;
  packet->data.sender_id = SENDER_ID;
  packet->data.count = 0;

  packet->data.measurements =
      (measurement_t *)vmalloc(sizeof(measurement_t) * MAX_MEASUREMENTS);

  if (packet->data.measurements == NULL) {
    vfree(packet);
    return -EFAULT;
  }

  // Allocate memory for send buffers
  primary_send_buffer = (uint8_t *)vmalloc(SEND_BUFFER_SIZE);
  primary_send_buffer_size = 0;
  if (primary_send_buffer == NULL) {
    vfree(packet->data.measurements);
    vfree(packet);
    return -EFAULT;
  }

  secondary_send_buffer = (uint8_t *)vmalloc(SEND_BUFFER_SIZE);
  secondary_send_buffer_size = 0;
  if (secondary_send_buffer == NULL) {
    vfree(primary_send_buffer);
    vfree(packet->data.measurements);
    vfree(packet);
    return -EFAULT;
  }

  prepare_for_sending();

  if (alloc_chrdev_region(&dev, 0, 1, DEVICE_NAME) < 0) {
    vfree(primary_send_buffer);
    vfree(secondary_send_buffer);
    vfree(packet->data.measurements);
    vfree(packet);
    printk(KERN_ALERT "Failed to allocate major number\n");
    return -1;
  }
  major_number = MAJOR(dev);

  class = class_create(CLASS_NAME);
  if (IS_ERR(class)) {
    vfree(primary_send_buffer);
    vfree(secondary_send_buffer);
    vfree(packet->data.measurements);
    vfree(packet);
    unregister_chrdev_region(MKDEV(major_number, 0), 1);
    printk(KERN_ALERT "Failed to create device class\n");
    return PTR_ERR(class);
  }

  if (device_create(class, NULL, MKDEV(major_number, 0), NULL, DEVICE_NAME) ==
      NULL) {
    vfree(primary_send_buffer);
    vfree(secondary_send_buffer);
    vfree(packet->data.measurements);
    vfree(packet);
    class_destroy(class);
    unregister_chrdev_region(MKDEV(major_number, 0), 1);
    printk(KERN_ALERT "Failed to create device\n");
    return -1;
  }

  cdev_init(&mycdev, &fops);
  if (cdev_add(&mycdev, MKDEV(major_number, 0), 1) < 0) {
    vfree(primary_send_buffer);
    vfree(secondary_send_buffer);
    vfree(packet->data.measurements);
    vfree(packet);
    device_destroy(class, MKDEV(major_number, 0));
    class_destroy(class);
    unregister_chrdev_region(MKDEV(major_number, 0), 1);
    printk(KERN_ALERT "Failed to add character device\n");
    return -1;
  }

  printk(KERN_INFO "lkm: device created successfully\n");

  if (gpio_request(GPIO_PIN, "GPIO_PIN") < 0) {
    vfree(primary_send_buffer);
    vfree(secondary_send_buffer);
    vfree(packet->data.measurements);
    vfree(packet);
    cdev_del(&mycdev);
    device_destroy(class, MKDEV(major_number, 0));
    class_destroy(class);
    unregister_chrdev_region(MKDEV(major_number, 0), 1);
    printk(KERN_ALERT "Failed to request GPIO pin\n");
    return -1;
  }
  gpio_direction_output(GPIO_PIN, 1);

  timed_thread = kthread_run(timed_thread_fn, NULL, "timed_thread");
  if (IS_ERR(timed_thread)) {
    vfree(primary_send_buffer);
    vfree(secondary_send_buffer);
    vfree(packet->data.measurements);
    vfree(packet);
    cdev_del(&mycdev);
    device_destroy(class, MKDEV(major_number, 0));
    class_destroy(class);
    unregister_chrdev_region(MKDEV(major_number, 0), 1);
    printk(KERN_ALERT "Failed to create timed thread\n");
    return -1;
  }

  interval = ktime_set(0, SLOT_INTERVAL_NS);
  hrtimer_init(&timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
  timer.function = timer_callback;
  hrtimer_start(&timer, interval, HRTIMER_MODE_REL);

  return 0;
}

static void __exit lkm_exit(void) {
  if (timed_thread) {
    kthread_stop(timed_thread);
  }

  hrtimer_cancel(&timer);

  mutex_destroy(&send_buffer_mutex);

  vfree(primary_send_buffer);
  vfree(secondary_send_buffer);
  vfree(packet->data.measurements);
  vfree(packet);
  cdev_del(&mycdev);
  device_destroy(class, MKDEV(major_number, 0));
  class_destroy(class);
  unregister_chrdev_region(MKDEV(major_number, 0), 1);
  printk(KERN_INFO "lkm: device removed successfully\n");

  gpio_set_value(GPIO_PIN, 0);
  gpio_free(GPIO_PIN);
}

module_init(lkm_init);
module_exit(lkm_exit);

MODULE_LICENSE("GPL");