1 files changed, 113 insertions, 0 deletions
diff --git a/modules/devices/cpubits.c b/modules/devices/cpubits.c
new file mode 100644
index 00000000..ba9bffc7
--- /dev/null
+++ b/modules/devices/cpubits.c
@@ -0,0 +1,113 @@
+/*
+ * rpiz - https://github.com/bp0/rpiz
+ * Copyright (C) 2017  Burt P. <pburt0@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ */
+
+#include <stdint.h>
+
+typedef uint32_t cpubits;
+uint32_t cpubits_count(cpubits *b);
+cpubits *cpubits_from_str(char *str);
+char *cpubits_to_str(cpubits *bits, char *str, int max_len);
+
+#define CPUBITS_SIZE 4096 /* bytes, multiple of sizeof(uint32_t) */
+#define CPUBIT_SET(BITS, BIT) (BITS[BIT/32] |= (1 << BIT%32))
+#define CPUBIT_GET(BITS, BIT) ((BITS[BIT/32] & (1 << BIT%32)) >> BIT%32)
+#define CPUBITS_CLEAR(BITS) memset(BITS, 0, CPUBITS_SIZE)
+
+uint32_t cpubits_count(cpubits *b) {
+    static const uint32_t max = CPUBITS_SIZE * 8;
+    uint32_t count = 0, i = 0;
+    while (i < max) {
+        count += CPUBIT_GET(b, i);
+        i++;
+    }
+    return count;
+}
+
+int cpubits_max(cpubits *b) {
+    int i = CPUBITS_SIZE * 8;
+    while (i >= 0) {
+        if (CPUBIT_GET(b, i))
+            break;
+        i--;
+    }
+    return i;
+}
+
+cpubits *cpubits_from_str(char *str) {
+    char *v, *nv, *hy;
+    int r0, r1;
+    cpubits *newbits = malloc(CPUBITS_SIZE);
+    if (newbits) {
+        memset(newbits, 0, CPUBITS_SIZE);
+        if (str != NULL) {
+            v = (char*)str;
+            while ( *v != 0 ) {
+                nv = strchr(v, ',');                /* strchrnul() */
+                if (nv == NULL) nv = strchr(v, 0);  /* equivalent  */
+                hy = strchr(v, '-');
+                if (hy && hy < nv) {
+                    r0 = strtol(v, NULL, 0);
+                    r1 = strtol(hy + 1, NULL, 0);
+                } else {
+                    r0 = r1 = strtol(v, NULL, 0);
+                }
+                for (; r0 <= r1; r0++) {
+                    CPUBIT_SET(newbits, r0);
+                }
+                v = (*nv == ',') ? nv + 1 : nv;
+            }
+        }
+    }
+    return newbits;
+}
+
+char *cpubits_to_str(cpubits *bits, char *str, int max_len) {
+    static const uint32_t max = CPUBITS_SIZE * 8;
+    uint32_t i = 1, seq_start = 0, seq_last = 0, seq = 0, l = 0;
+    char buffer[65536] = "";
+    if (CPUBIT_GET(bits, 0)) {
+        seq = 1;
+        strcpy(buffer, "0");
+    }
+    while (i < max) {
+        if (CPUBIT_GET(bits, i) ) {
+            seq_last = i;
+            if (!seq) {
+                seq = 1;
+                seq_start = i;
+                l = strlen(buffer);
+                sprintf(buffer + l, "%s%d", l ? "," : "", i);
+            }
+        } else {
+            if (seq && seq_last != seq_start) {
+                l = strlen(buffer);
+                sprintf(buffer + l, "-%d", seq_last);
+            }
+            seq = 0;
+        }
+        i++;
+    }
+    if (str == NULL)
+        return strdup(buffer);
+    else {
+        strncpy(str, buffer, max_len);
+        return str;
+    }
+}