From 15d9095122c0d4f9a4dab05fb0e84d84c624e1b4 Mon Sep 17 00:00:00 2001
From: PA4WDH
Date: Sat, 20 May 2023 17:17:43 +0200
Subject: Translate POC to C code

---
 .gitignore          |   3 +
 Makefile            |   8 +++
 cpufreq.c           |  82 +++++++++++++++++++++
 cpufreq.h           |   2 +
 cputemp2maxfreq.c   |  75 ++++++++++++++++++++
 debug.c             |  71 +++++++++++++++++++
 generate_debug_h.sh | 201 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 sysfs.c             |  83 ++++++++++++++++++++++
 sysfs.h             |   3 +
 9 files changed, 528 insertions(+)
 create mode 100644 .gitignore
 create mode 100644 Makefile
 create mode 100644 cpufreq.c
 create mode 100644 cpufreq.h
 create mode 100644 cputemp2maxfreq.c
 create mode 100644 debug.c
 create mode 100755 generate_debug_h.sh
 create mode 100644 sysfs.c
 create mode 100644 sysfs.h

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..7b39e57
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,3 @@
+*.o
+cputemp2maxfreq
+debug.h
diff --git a/Makefile b/Makefile
new file mode 100644
index 0000000..4c968ee
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,8 @@
+cputemp2maxfreq: debug.o sysfs.o cpufreq.o
+
+%.o: %.c %.h debug.h
+	$(COMPILE.c) $(OUTPUT_OPTION) $<
+
+debug.h: generate_debug_h.sh debug.c
+	./generate_debug_h.sh > debug.h.new
+	mv debug.h.new debug.h
diff --git a/cpufreq.c b/cpufreq.c
new file mode 100644
index 0000000..d9e0e4e
--- /dev/null
+++ b/cpufreq.c
@@ -0,0 +1,82 @@
+#include <fcntl.h>
+#include <sys/syscall.h>
+#include <unistd.h>
+#include <string.h>
+#include <stdio.h>
+#include "debug.h"
+#include "sysfs.h"
+
+#define DIR_BUF_SIZE 1024
+
+int cpufreq_is_cpu(char *name)
+{
+ if (strncmp(name,"cpu",3)!=0) return 0;
+ if ((name[3]<'0') || (name[3]>'9')) return 0;
+
+ return 1;
+}
+
+int cpufreq_set_str(char *parameter,char *value,int checkdelay)
+{
+ int fd;
+ char sysfs_file[128];
+
+ DEBUG1_CPUFREQ("Set %s to %s\n",parameter,value)
+
+ char buf[DIR_BUF_SIZE];
+ long nread;
+
+ struct linux_dirent {
+  unsigned long d_ino;
+  off_t d_off;
+  unsigned short d_reclen;
+  char d_name[];
+ };
+
+ struct linux_dirent *d;
+ size_t bpos;
+
+ fd=open("/sys/devices/system/cpu",O_RDONLY|O_DIRECTORY);
+ if (fd==-1)
+ {
+  DEBUG1_CPUFREQ("Unable to find CPU's\n");
+  return -1;
+ }
+
+ while(1)
+ {
+  nread=syscall(SYS_getdents,fd,buf,DIR_BUF_SIZE);
+  if (nread==-1)
+  {
+   DEBUG1_CPUFREQ("Unable to find CPU's\n");
+   close(fd);
+   return -1;
+  }
+  if (nread==0) break;
+
+  for(bpos=0;bpos<nread;)
+  {
+   d=(struct linux_dirent *)(buf+bpos);
+   if (cpufreq_is_cpu(d->d_name))
+   {
+    DEBUG2_CPUFREQ("Found CPU %s\n",d->d_name);
+    snprintf(sysfs_file,128,"/sys/devices/system/cpu/%s/cpufreq/%s",d->d_name,parameter);
+    if (sysfs_write_str(sysfs_file,value,checkdelay)!=0)
+    {
+     DEBUG1_CPUFREQ("Failed to set %s\n",d->d_name);
+     return -1;
+    }
+   }
+   bpos+=d->d_reclen;
+  } 
+ }
+ close(fd);
+}
+
+int cpufreq_set_long_int(char *parameter,long int value,int checkdelay)
+{
+ char buf[255];
+
+ snprintf(buf,255,"%ld",value);
+ cpufreq_set_str(parameter,buf,checkdelay);
+}
diff --git a/cpufreq.h b/cpufreq.h
new file mode 100644
index 0000000..f950339
--- /dev/null
+++ b/cpufreq.h
@@ -0,0 +1,2 @@
+int cpufreq_set_str(char *parameter,char *value,int checkdelay);
+int cpufreq_set_long_int(char *parameter,long int value,int checkdelay);
diff --git a/cputemp2maxfreq.c b/cputemp2maxfreq.c
new file mode 100644
index 0000000..1760996
--- /dev/null
+++ b/cputemp2maxfreq.c
@@ -0,0 +1,75 @@
+#include <stdio.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include "sysfs.h"
+#include "cpufreq.h"
+#include "debug.h"
+
+struct s_cpudata {
+ long int min_freq;
+ long int max_freq;
+ long int cur_freq;
+ long int cur_temp;
+ long int scale_max;
+};
+
+struct s_config {
+ char *governor;
+ long int max_temp;
+ char *temp_input;
+ long int freq_step;
+ long int fallback_freq;
+ unsigned int interval;
+};
+
+struct s_cpudata cpudata;
+struct s_config config={"conservative",70000,"/sys/devices/virtual/thermal/thermal_zone0/temp",100000,2000000,10};
+
+int main()
+{
+ long int diff;
+ long int newfreq;
+ 
+ printf("Governor: %s\nTemp input: %s\n",config.governor,config.temp_input);
+
+ if (cpufreq_set_str("scaling_governor",config.governor,0)!=0)
+ {
+  printf("Failed to set governow\n");
+  exit(1);
+ }
+
+ cpudata.min_freq=sysfs_read_long_int("/sys/devices/system/cpu/cpufreq/policy0/cpuinfo_min_freq");
+ cpudata.max_freq=sysfs_read_long_int("/sys/devices/system/cpu/cpufreq/policy0/cpuinfo_max_freq");
+ cpudata.scale_max=sysfs_read_long_int("/sys/devices/system/cpu/cpufreq/policy0/scaling_max_freq");
+
+ while(1)
+ {
+  cpudata.cur_freq=sysfs_read_long_int("/sys/devices/system/cpu/cpufreq/policy0/scaling_cur_freq");
+  cpudata.cur_temp=sysfs_read_long_int(config.temp_input);
+
+
+  DEBUG1_MAIN("Data: %ld %ld %ld %ld %ld\n",cpudata.cur_temp,config.max_temp,cpudata.max_freq,cpudata.scale_max,cpudata.cur_freq);
+
+  if ((cpudata.cur_temp<config.max_temp) && (cpudata.scale_max<cpudata.max_freq))
+  {
+   diff=(config.max_temp-cpudata.cur_temp)/1000;
+   newfreq=cpudata.scale_max+(config.freq_step*diff);
+   if (newfreq>cpudata.max_freq) newfreq=cpudata.max_freq;
+   DEBUG1_MAIN("Increase to %ld\n",newfreq);
+   cpufreq_set_long_int("scaling_max_freq",newfreq,100);
+   cpudata.scale_max=sysfs_read_long_int("/sys/devices/system/cpu/cpufreq/policy0/scaling_max_freq");
+  }
+
+  if ((cpudata.cur_temp>config.max_temp) && (cpudata.scale_max>cpudata.min_freq))
+  {
+   diff=(cpudata.cur_temp-config.max_temp)/1000;
+   newfreq=cpudata.scale_max-(config.freq_step*diff);
+   if (newfreq<cpudata.min_freq) newfreq=cpudata.min_freq;
+   DEBUG1_MAIN("Decrease to %ld\n",newfreq);
+   cpufreq_set_long_int("scaling_max_freq",newfreq,100);
+   cpudata.scale_max=sysfs_read_long_int("/sys/devices/system/cpu/cpufreq/policy0/scaling_max_freq");
+  }
+
+  sleep(config.interval);
+ }
+}
diff --git a/debug.c b/debug.c
new file mode 100644
index 0000000..b319b57
--- /dev/null
+++ b/debug.c
@@ -0,0 +1,71 @@
+#include "debug.h"
+
+#ifdef NEED_DEBUG_HELPER
+#include <stdio.h>
+#include <time.h>
+#include <sys/time.h>
+
+void debug_print_time()
+{
+ struct timeval debug_tv;
+ struct tm *debug_tm;
+ char debug_buffer[10];
+
+ gettimeofday(&debug_tv,NULL);
+ debug_tm=localtime(&debug_tv.tv_sec);
+ strftime(debug_buffer,10, "%T",debug_tm);
+ fprintf(stderr,"%s.%06d: ",debug_buffer,(int)debug_tv.tv_usec);
+}
+
+#endif
+
+#ifdef NEED_HEXDUMP_HELPER
+#include <stdio.h>
+
+void hexdump(unsigned char *data,int len)
+{
+ int count;
+ int count2;
+ int max;
+
+ for(count=0;count<len;count+=16)
+ {
+#ifdef SET_DEBUG_PRINT_PREFIX
+  fprintf(stderr,"%s",SET_DEBUG_PRINT_PREFIX);
+#endif
+  fprintf(stderr,"%04x  ",count);
+
+  max=count+16;
+  if (max>len) max=len;
+
+  for(count2=count;count2<max;count2++)
+  {
+   if (count2%8==0) fprintf(stderr," ");
+   fprintf(stderr,"%02x ",data[count2]);
+  }
+
+  if (max<count+16)
+  {
+   for(count2=max;count2<count+16;count2++)
+   {
+    if (count2%8==0) fprintf(stderr," ");
+    fprintf(stderr,"   ");
+   }
+  }
+  fprintf(stderr,"  ");
+
+  for(count2=count;count2<max;count2++)
+  {
+   if (count2%8==0) fprintf(stderr," ");
+   if ((data[count2]>=32) && (data[count2]<127))
+   {
+    fprintf(stderr,"%c",data[count2]);
+   } else {
+    fprintf(stderr,".");
+   }
+  }
+
+  fprintf(stderr,"\n");
+ }
+}
+#endif
diff --git a/generate_debug_h.sh b/generate_debug_h.sh
new file mode 100755
index 0000000..ee1f976
--- /dev/null
+++ b/generate_debug_h.sh
@@ -0,0 +1,201 @@
+#!/bin/sh
+#
+# Script to generate debug.h
+# Output goes to stdout, this should be redirected by the caller
+
+# Configuration
+DEBUG_LEVELS=4
+DEBUG_PARTS="MAIN SYSFS CPUFREQ"
+DEBUG_HELPER_DEF=`grep -o "void .*()" debug.c | head -n 1`
+DEBUG_HELPER=${DEBUG_HELPER_DEF##* }
+HEXDUMP_HELPER_DEF=`grep -o "void .*(unsigned char \*data,int len)" debug.c`
+HEXDUMP_HELPER=${HEXDUMP_HELPER_DEF#* }
+HEXDUMP_HELPER=${HEXDUMP_HELPER%%(*}
+
+# Generate the configuration part
+cat << EOF
+#ifndef HAVE_DEBUG_H
+#define HAVE_DEBUG_H
+
+// Configuration
+EOF
+
+for PART in ALL $DEBUG_PARTS
+do
+ VALUE=0
+ if [ -r "debug.h" ]
+ then
+  VALUE=`grep "^#define SET_DEBUG_$PART " debug.h | head -n 1 | awk '{ print $NF }' | tr -cd "[0-9]"`
+ fi
+ if [ -n "$VALUE" ]
+ then
+  echo "#define SET_DEBUG_$PART $VALUE"
+ else
+  echo "#define SET_DEBUG_$PART 0"
+ fi
+done
+
+for SETTING in SET_DEBUG_PRINT_LEVEL SET_DEBUG_PRINT_TIME SET_DEBUG_PRINT_LINE SET_DEBUG_PRINT_FUNCTION
+do
+ if [ -r "debug.h" ]
+ then
+  if grep -q "^#define $SETTING" debug.h
+  then
+   echo "#define $SETTING"
+  else
+   echo "//#define $SETTING"
+  fi
+ else
+  echo "#define $SETTING"
+ fi
+done
+echo "// End of configuration"
+echo ""
+
+# Configure prefix
+echo "// Set prefix"
+if [ -n "$DEBUG_PREFIX_CONDITION" ]
+then
+ echo "$DEBUG_PREFIX_CONDITION"
+fi
+echo "#define SET_DEBUG_PRINT_PREFIX \"$DEBUG_PREFIX\""
+if [ -n "$DEBUG_PREFIX_CONDITION" ]
+then
+ echo "#endif"
+fi
+echo ""
+
+# Generate the part to make ALL work
+echo "// Make ALL work"
+for PART in $DEBUG_PARTS
+do
+ echo "#if (SET_DEBUG_ALL>SET_DEBUG_$PART)"
+ echo " #undef SET_DEBUG_$PART"
+ echo " #define SET_DEBUG_$PART SET_DEBUG_ALL"
+ echo "#endif"
+ echo ""
+done
+
+# Check if we need stdio pr the helper
+echo "// Check if we need stdio.h or the helpers"
+for PART in $DEBUG_PARTS
+do
+ echo "#if (SET_DEBUG_$PART>0)"
+ echo " #ifndef NEED_STDIO_H"
+ echo "  #define NEED_STDIO_H"
+ echo " #endif"
+ echo " #ifdef SET_DEBUG_PRINT_TIME"
+# echo "  #ifdef NEED_DEBUG_HELPER"
+# echo "   #undef NEED_DEBUG_HELPER"
+# echo "  #endif"
+ echo "  #ifndef NEED_DEBUG_HELPER"
+ echo "   #define NEED_DEBUG_HELPER"
+ echo "  #endif"
+ echo " #endif"
+ echo " #ifndef NEED_HEXDUMP_HELPER"
+ echo "  #define NEED_HEXDUMP_HELPER"
+ echo " #endif"
+ echo "#endif"
+ echo ""
+done
+
+# Print the format definitions
+# Define printing of prefixes
+cat <<EOF
+// Define the printing of the debug prefix
+#ifdef SET_DEBUG_PRINT_PREFIX
+ #define DEBUG_PRINT_PREFIX fprintf(stderr,"%s",SET_DEBUG_PRINT_PREFIX);
+#else
+ #define DEBUG_PRINT_PREFIX
+#endif
+
+EOF
+# Generate the ammount of levels requested
+echo "// Define the printing of debuglevels"
+echo "#ifdef SET_DEBUG_PRINT_LEVEL"
+for((COUNT=1;COUNT<=$DEBUG_LEVELS;COUNT++))
+do
+ echo " #define DEBUG_PRINT_LEVEL$COUNT fprintf(stderr,\"$COUNT: \");"
+done
+echo "#else"
+for((COUNT=1;COUNT<=$DEBUG_LEVELS;COUNT++))
+do
+ echo " #define DEBUG_PRINT_LEVEL$COUNT"
+done
+echo "#endif"
+echo ""
+
+echo "// Debug timestamps definition"
+echo "#ifdef SET_DEBUG_PRINT_TIME"
+#echo "#define NEED_DEBUG_HELPER"
+echo " #define DEBUG_PRINT_TIME $DEBUG_HELPER;"
+echo "#else"
+echo " #define DEBUG_PRINT_TIME"
+echo "#endif"
+echo ""
+
+cat << EOF
+// Define the printing of file names and line numbers
+#ifdef SET_DEBUG_PRINT_LINE
+ #define DEBUG_PRINT_LINE fprintf(stderr,"%s:%d: ",__FILE__,__LINE__);
+#else
+ #define DEBUG_PRINT_LINE
+#endif
+
+// Define the printing of function names
+#ifdef SET_DEBUG_PRINT_FUNCTION
+ #define DEBUG_PRINT_FUNCTION fprintf(stderr,"%s: ",__func__);
+#else
+ #define DEBUG_PRINT_FUNCTION
+#endif
+
+EOF
+
+echo "// Define debugging format"
+for((COUNT=1;COUNT<=$DEBUG_LEVELS;COUNT++))
+do
+ echo "#define DEBUG${COUNT}_FORMAT(...) \\"
+ echo " DEBUG_PRINT_PREFIX \\"
+ echo " DEBUG_PRINT_LEVEL${COUNT} \\"
+ echo " DEBUG_PRINT_TIME \\"
+ echo " DEBUG_PRINT_LINE \\"
+ echo " DEBUG_PRINT_FUNCTION \\"
+ echo " fprintf(stderr,__VA_ARGS__);"
+ echo ""
+done
+
+# Generate the debug functions
+echo "// Define the functions"
+for PART in $DEBUG_PARTS
+do
+ for((COUNT=1;COUNT<=$DEBUG_LEVELS;COUNT++))
+ do
+  echo "#if (SET_DEBUG_$PART>$((COUNT-1)))"
+  echo " #define DEBUG${COUNT}_$PART(...) DEBUG${COUNT}_FORMAT(__VA_ARGS__)"
+  echo " #define HEXDUMP${COUNT}_$PART(data,datalen) $HEXDUMP_HELPER(data,datalen);"
+  echo "#else"
+  echo " #define DEBUG${COUNT}_$PART(...)"
+  echo " #define HEXDUMP${COUNT}_$PART(data,datalen)"
+  echo "#endif"
+  echo ""
+ done
+done
+
+# Include stdio when needed
+echo "#ifdef NEED_STDIO_H"
+echo " #include <stdio.h>"
+echo "#endif"
+
+# Define helper function
+echo "#ifdef NEED_DEBUG_HELPER"
+echo " $DEBUG_HELPER_DEF;"
+echo "#endif"
+
+# Define hexdump function
+echo "#ifdef NEED_HEXDUMP_HELPER"
+echo " $HEXDUMP_HELPER_DEF;"
+echo "#endif"
+
+# Finish the ifdef HAVE_DEBUG_H
+echo ""
+echo "#endif"
diff --git a/sysfs.c b/sysfs.c
new file mode 100644
index 0000000..e31aef2
--- /dev/null
+++ b/sysfs.c
@@ -0,0 +1,83 @@
+#include <stdio.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include "debug.h"
+
+int sysfs_read_str(char *sysfs_file,char *buf,size_t bufsize)
+{
+ int sysfs_handle;
+ size_t datalen;
+
+ DEBUG1_SYSFS("Reading sysfs file %s\n",sysfs_file);
+
+// Open the file
+ sysfs_handle=open(sysfs_file,O_RDONLY);
+ DEBUG2_SYSFS("Open returned %d\n",sysfs_handle);
+ if (sysfs_handle<0) return -1;
+
+// Read the content of the file
+ datalen=read(sysfs_handle,buf,bufsize);
+ HEXDUMP2_SYSFS(buf,datalen);
+ close(sysfs_handle);
+ buf[datalen]=0;
+
+// Strip newline
+ if (buf[datalen-1]=='\n')
+ {
+  DEBUG2_SYSFS("Newline stripped\n");
+  datalen--;
+  buf[datalen]=0;
+ }
+
+ return datalen;
+}
+
+long int sysfs_read_long_int(char *sysfs_file)
+{
+ char buf[255];
+ int datalen;
+
+// Read data
+ datalen=sysfs_read_str(sysfs_file,buf,sizeof(buf));
+ if (datalen<0) return 0;
+
+// Convert to long int
+ return strtoll((char*)&buf,NULL,10);
+}
+
+int sysfs_write_str(char *sysfs_file,char *value,int checkdelay)
+{
+ int sysfs_handle;
+ size_t len;
+ char buf[255];
+
+ DEBUG1_SYSFS("Writing %s to sysfs file %s\n",value,sysfs_file);
+
+// Open the file
+ sysfs_handle=open(sysfs_file,O_WRONLY);
+ DEBUG2_SYSFS("Open returned %d\n",sysfs_handle);
+ if (sysfs_handle<0) return -1;
+
+// Write the data
+ len=write(sysfs_handle,value,strlen(value));
+ close(sysfs_handle);
+ DEBUG2_SYSFS("Written %zd bytes\n",len);
+
+// What if instructed to
+ if (checkdelay>0) usleep(checkdelay*1000);
+
+// Validate we actually set what we want to set
+ sysfs_read_str(sysfs_file,buf,sizeof(buf));
+ DEBUG2_SYSFS("Content is now %s\n",buf);
+
+ if (strncmp(value,buf,255)!=0)
+ {
+  DEBUG2_SYSFS("Set failed\n");
+  return -1;
+ }
+ DEBUG2_SYSFS("Set successfull\n");
+
+ return 0;
+}
diff --git a/sysfs.h b/sysfs.h
new file mode 100644
index 0000000..ccd2430
--- /dev/null
+++ b/sysfs.h
@@ -0,0 +1,3 @@
+int sysfs_read_str(char *sysfs_file,char *buf,size_t bufsize);
+long int sysfs_read_long_int(char *sysfs_file);
+int sysfs_write_str(char *sysfs_file,char *value,int checkdelay);
-- 
cgit v1.2.3