softirq.c

#include "main-header.h"
#include "softirq.h"
#include "user_app_exe.h"
 
#define MCULIB_SOFTIRQ_HANDLE 13
#define MAX_SOFTIRQS 10
 
static int hw_timer;
static uint32_t real_irq_hertz;
static uint8_t softirqs_enabled;
 
typedef struct softirqdef {
  void (*callback)(uint32_t); // if null, entry is not used
  uint32_t      hertz;
  uint32_t      opaque;
  uint32_t      timer_skip;     // how many timer real irqs to skip before calling this
  uint32_t      timer_skipctr;  // counter from timer_skip to zero
  uint32_t      invoke_ctr;
} _softirqdef;
 
static struct softirqdef softirqs[MAX_SOFTIRQS];
 
void softirq_init() {
  memset(softirqs, 0, sizeof(softirqs));
  softirqs_enabled = 0;
  real_irq_hertz = 0;
}
 
static struct softirqdef *find_free() {
  int i;
 
  for (i = 0; i < MAX_SOFTIRQS; i++) {
    struct softirqdef *res = &softirqs[i];
    if (res->callback == NULL) {
      return res;
    }
  }
  return NULL;
}
 
static void softirq_real_irq() {
  int i;
 
  for (i = 0; i < MAX_SOFTIRQS; i++) {
    struct softirqdef *res = &softirqs[i];
    void (*c)(uint32_t);
    c = res->callback;
    if (c == NULL) {
      continue;
    }
    if (res->timer_skipctr <= 1) {
      res->timer_skipctr = res->timer_skip;
      set_in_app(1); // too slow
      res->invoke_ctr++;
      c(res->opaque);
      set_in_app(0); // too slow
      continue;
    }
    res->timer_skipctr--;
  }
}
 
int softirq_set_real_frequency(uint32_t hertz) {
  int r;
 
  if (hertz == real_irq_hertz) {
    return 0;
  }
  if (hertz == 0) {
    printf("[softirq] disabling timer %i\r\n", hw_timer);
    r = mculib_timer_disable(MCULIB_SOFTIRQ_HANDLE, hw_timer);
    if (r != 0) {
      printf("[softirq] failed to disable timer %i: %i\r\n", hw_timer, r);
    } else {
      printf("[softirq] disabled timer %i: %i\r\n", hw_timer);
      real_irq_hertz = 0;
      return 0;
    }
  }
  // leds use either timer 1 or timer 3
  int timer = 1;
 
  if (config_get_flag(CONFIG_FLAGS_LEDEXTERN)) {
    timer = 3;
  }
  hw_timer = timer;
 
  printf("[softirq] changing frequency of timer %i to %iHz\r\n", hw_timer, hertz);
  r = mculib_timer_enable_simple(MCULIB_SOFTIRQ_HANDLE, timer, hertz, &softirq_real_irq);
  if (r != 0) {
    printf("[softirq] failed to change timer: %i\r\n", r);
    return 1;
  }
 
  real_irq_hertz = hertz;
  return 0;
}
 
 
void softirqs_removeall() {
  softirqs_enabled = 0;
  int i;
 
  for (i = 0; i < MAX_SOFTIRQS; i++) {
    struct softirqdef *res = &softirqs[i];
    res->callback = NULL;
  }
  softirq_set_real_frequency(0);
  softirqs_enabled = 1;
}
 
 
 
// calculate timer_skip values based on current "real_irq_hertz"
void softirq_calc_settings() {
  int i;
 
  for (i = 0; i < MAX_SOFTIRQS; i++) {
    struct softirqdef *res = &softirqs[i];
    if (res->callback == NULL) {
      continue;
    }
    res->timer_skip = real_irq_hertz / res->hertz;
    res->timer_skipctr = res->timer_skip;
  }
}
 
void print_softirqs() {
  int i;
 
  printf("[softirq] Hardware IRQ, timer %i, running at %iHz, enabled=%i\r\n", hw_timer, real_irq_hertz, softirqs_enabled);
  int j = 0;
 
  for (i = 0; i < MAX_SOFTIRQS; i++) {
    struct softirqdef *res = &softirqs[i];
    if (res->callback == NULL) {
      continue;
    }
    j++;
    printf("[softirq] IRQ #%i running at %iHz, skip=%i, skipctr=%i,opaque=%i,invoked=%i\r\n", i, res->hertz, res->timer_skip, res->timer_skipctr, res->opaque, res->invoke_ctr);
  }
  ;
  printf("[softirq] %i softirqs active\r\n", j);
}
 
 
 
// add a softirq callback
int softirq_timer(uint32_t hertz, uint32_t opaque, void (*callback)(uint32_t)) {
  printf("[softirq] adding irq with %iHz (hardware timer @%iHz)\r\n", hertz, real_irq_hertz);
  struct softirqdef *sid = find_free();
 
  if (sid == NULL) {
    printf("[softirq] could not add irq: maximum softirq entries (%i) exceeded\r\n", MAX_SOFTIRQS);
    return 1;
  }
  softirqs_enabled = 0;
  // TODO: implement an LCM here
  // example: https://www.geeksforgeeks.org/program-to-find-lcm-of-two-numbers/
  if (hertz > real_irq_hertz) {
    if (softirq_set_real_frequency(hertz) != 0) {
      printf("[softirq] could not add irq: failed to set hardware irq to %iHz\r\n", sid->hertz);
      softirqs_enabled = 1;
      return 2;
    }
  }
 
  sid->callback = callback;
  sid->hertz = hertz;
  sid->opaque = opaque;
  sid->invoke_ctr = 0;
  softirq_calc_settings();
  softirqs_enabled = 1;
  printf("[softirq] added irq with %iHz (opaque=%i)\r\n", sid->hertz, sid->opaque);
  return 0;
}