Quantron fix rtprio patch
Posted by Anonymous on Tue 22nd Oct 2019 16:41
raw | new post

diff --git a/ptxdist/local_src/ecu01-codesys/io_driver/ecu01.cpp b/ptxdist/local_src/ecu01-codesys/io_driver/ecu01.cpp
index 4148859..bf56618 100644
--- a/ptxdist/local_src/ecu01-codesys/io_driver/ecu01.cpp
+++ b/ptxdist/local_src/ecu01-codesys/io_driver/ecu01.cpp
@@ -144,7 +144,7 @@ void EcuDriverInit(char *pcDeviceName, int iInputCnt, int iOutputCnt)
       ready = false;
    }

-   threadHdlCC = CAL_SysTaskCreate( (char*)threadHdlCCName, threadCC, NULL, TASKPRIO_SYSTEM_BASE+42, 0, 0, NULL, &rtsResult);
+   threadHdlCC = CAL_SysTaskCreate( (char*)threadHdlCCName, threadCC, NULL, TASKPRIO_SYSTEM_BASE+6, 0, 0, NULL, &rtsResult);
    if ((threadHdlCC == RTS_INVALID_HANDLE) || (rtsResult != ERR_OK)) {
       /* error */
       ready = false;
diff --git a/ptxdist/local_src/ecu01-codesys/io_driver/ecu01softcompcap.cpp b/ptxdist/local_src/ecu01-codesys/io_driver/ecu01softcompcap.cpp
index 8dea629..7085534 100644
--- a/ptxdist/local_src/ecu01-codesys/io_driver/ecu01softcompcap.cpp
+++ b/ptxdist/local_src/ecu01-codesys/io_driver/ecu01softcompcap.cpp
@@ -31,6 +31,7 @@
 #include "ecu01softcompcap.h"
 #include <time.h>
 #include <limits.h>
+#include <stdlib.h>
 #include <stdio.h>
 #include <string.h>

@@ -255,11 +256,15 @@ Ecu01SoftCompCap::Ecu01SoftCompCap():
    _callback120(120),
    _statusFlag(1)
 {
-   int prio = sched_get_priority_max(SCHED_FIFO);
+   /*
+    * Note: MAX_USER_RT_PRIO is 100, while sched_get_priority_max(SCHED_FIFO)
+    * in userland returns 99
+    */
+   int softcc_prio = sched_get_priority_max(SCHED_FIFO)-6;
    struct sched_param param;

    (void)memset(&param, 0, sizeof(param));
-   param.sched_priority = prio;
+   param.sched_priority = softcc_prio;

    if( _gpio120.isOpen() ){
       if( _gpio120.guardGpioChange( &_callback120, 1000 ) ){
@@ -269,6 +274,19 @@ Ecu01SoftCompCap::Ecu01SoftCompCap():
          }
       }
    }
+
+   /*
+    * Bump priority of the gpiolib irq handling thread.
+    *
+    * It seems there is no way to force individual gpios their own
+    * threads with a different realtime priority, so we have
+    * to do this here, globally for all gpios.
+    *
+    * WARNING: This is very likely causing trouble elsewhere,
+    * because now ALL gpiolib activity runs with a glibgpio
+    * irq handling thread with priority 97!!
+    */
+   (void)system("x=$(/usr/bin/pgrep 'irq/.+-gpiolib') && /usr/bin/chrt -f -p 97 \"$x\"");
 }

 // ****************************************************************************
diff --git a/ptxdist/local_src/ecu01-comp-cap/main.c b/ptxdist/local_src/ecu01-comp-cap/main.c
index 1fe9651..787565b 100644
--- a/ptxdist/local_src/ecu01-comp-cap/main.c
+++ b/ptxdist/local_src/ecu01-comp-cap/main.c
@@ -47,7 +47,11 @@
 #include <linux/seq_file.h>
 #include <linux/cdev.h>
 #include <linux/spinlock.h>
-
+#include <linux/irq.h>
+#include <linux/irqdesc.h>
+#include <linux/interrupt.h>
+#include <linux/sched.h>
+#include <linux/sched/rt.h>
 //#include <asm/system.h>         /* cli(), *_flags */
 #include <asm/uaccess.h>        /* copy_*_user */

@@ -177,6 +181,85 @@ int comp_cap_release(struct inode *inode, struct file *filp)
    return 0;
 }

+static
+struct task_struct *irq_desc_to_irq_thread_task_struct(struct irq_desc *irq_desc)
+{
+       if (irq_desc && irq_desc->action)
+       {
+               /*
+                * Note: |irq_desc->action| can be chained if the irq
+                * is a shared one. Maybe we should place a fatal assert
+                * for that case since we do not implement this here.
+                */
+               return irq_desc->action->thread;
+       }
+
+       return NULL;
+}
+
+/*
+ * 1. Note: MAX_USER_RT_PRIO is 100, while sched_get_priority_max(SCHED_FIFO)
+ *   in userland returns 99
+ * 2. The kernel thread "posixcputmr/0" must run with rtprio 99, and we
+ *   MUST NOT interfere with it!!
+ */
+#define ECU01_CC_IRQ_THREAD_RTPRIO (MAX_USER_RT_PRIO-5)
+
+/*
+ * set scheduler paramters of the compare capture interrupt threads
+ *
+ * Note: Realtime priorities of the compare capture interrupt
+ * threads can be monitored with
+ * $ ps -eT -o s,tid,pid,cls,pri,rtprio,comm,time | egrep 'irq/.+CC' #
+ *
+ * ToDo: Failure to set the parameters should be fatal for the
+ * |open()| syscall which calls this function!
+ */
+static
+void set_cc_irq_thread_scheduler_parameters(struct comp_cap_dev_priv *priv_dev)
+{
+       struct irq_desc *irq_desc;
+       bool set_sched_success = false;
+       struct task_struct *ts;
+       struct sched_param param;
+       (void)memset(&param, 0, sizeof(param));
+       param.sched_priority = ECU01_CC_IRQ_THREAD_RTPRIO;
+
+       irq_desc = irq_to_desc(priv_dev->IRQ);
+       if (!irq_desc)
+       {
+               /* This can fail with certain kernel CONFIG_* settings */
+               (void)printk(KERN_EMERG "%s:%d: %s\n",
+                       __func__, __LINE__, "irq_to_desc() failed.");
+               return;
+       }
+
+       ts = irq_desc_to_irq_thread_task_struct(irq_desc);
+       if (ts)
+       {
+               if (sched_setscheduler(ts, SCHED_FIFO, &param) == 0)
+                       set_sched_success = true;
+       }
+
+       /*
+        * Be very verbose&loud about setting the scheduler class
+        * and priority, since the application will not work correctly
+        * if we fail to do this.
+        *
+        * ToDo: Propagate an error to the caller, so that a device
+        * |open()| FAILS.
+        */
+       (void)printk(KERN_EMERG "ecu01-compcap: "
+               "irq=%ld/%ld, %s interrupt thread pid=%ld to '%s'/rtprio=%ld\n",
+               (long)irq_desc->irq_data.irq,
+               (long)priv_dev->IRQ,
+               (set_sched_success?"changed":"failed to change"),
+               (long)(ts?ts->pid:-1),
+               "SCHED_FIFO",
+               (long)param.sched_priority);
+}
+
+
 int start_comp_cap( struct comp_cap_dev *dev, int enable )
 {
    int res = -1;
@@ -188,6 +271,8 @@ int start_comp_cap( struct comp_cap_dev *dev, int enable )

    res = request_irq(priv_dev->IRQ, cc_isr, IRQF_DISABLED, dev->NAME , priv_dev );

+   set_cc_irq_thread_scheduler_parameters(priv_dev);
+
    cc_restart(priv_dev);

    priv_dev->clk = clk_get_sys( priv_dev->CLK_PROVIDER, priv_dev->CLK_PARENT);