1 files changed, 402 insertions, 0 deletions

diff --git a/libblkid/cpuset.c b/libblkid/cpuset.c
new file mode 100644
index 000000000..e5b6b9dfe
--- /dev/null
+++ b/libblkid/cpuset.c
@@ -0,0 +1,402 @@
+/*
+ * Terminology:
+ *
+ *	cpuset	- (libc) cpu_set_t data structure represents set of CPUs
+ *	cpumask	- string with hex mask (e.g. "0x00000001")
+ *	cpulist - string with CPU ranges (e.g. "0-3,5,7,8")
+ *
+ * Based on code from taskset.c and Linux kernel.
+ *
+ * This file may be redistributed under the terms of the
+ * GNU Lesser General Public License.
+ *
+ * Copyright (C) 2010 Karel Zak <kzak@redhat.com>
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <sched.h>
+#include <errno.h>
+#include <string.h>
+#include <ctype.h>
+#include <sys/syscall.h>
+
+#include "cpuset.h"
+#include "c.h"
+
+static inline int val_to_char(int v)
+{
+	if (v >= 0 && v < 10)
+		return '0' + v;
+	else if (v >= 10 && v < 16)
+		return ('a' - 10) + v;
+	else
+		return -1;
+}
+
+static inline int char_to_val(int c)
+{
+	int cl;
+
+	cl = tolower(c);
+	if (c >= '0' && c <= '9')
+		return c - '0';
+	else if (cl >= 'a' && cl <= 'f')
+		return cl + (10 - 'a');
+	else
+		return -1;
+}
+
+static const char *nexttoken(const char *q,  int sep)
+{
+	if (q)
+		q = strchr(q, sep);
+	if (q)
+		q++;
+	return q;
+}
+
+/*
+ * Number of bits in a CPU bitmask on current system
+ */
+int get_max_number_of_cpus(void)
+{
+#ifdef SYS_sched_getaffinity
+	int n, cpus = 2048;
+	size_t setsize;
+	cpu_set_t *set = cpuset_alloc(cpus, &setsize, NULL);
+
+	if (!set)
+		return -1;	/* error */
+
+	for (;;) {
+		CPU_ZERO_S(setsize, set);
+
+		/* the library version does not return size of cpumask_t */
+		n = syscall(SYS_sched_getaffinity, 0, setsize, set);
+
+		if (n < 0 && errno == EINVAL && cpus < 1024 * 1024) {
+			cpuset_free(set);
+			cpus *= 2;
+			set = cpuset_alloc(cpus, &setsize, NULL);
+			if (!set)
+				return -1;	/* error */
+			continue;
+		}
+		cpuset_free(set);
+		return n * 8;
+	}
+#endif
+	return -1;
+}
+
+/*
+ * Allocates a new set for ncpus and returns size in bytes and size in bits
+ */
+cpu_set_t *cpuset_alloc(int ncpus, size_t *setsize, size_t *nbits)
+{
+	cpu_set_t *set = CPU_ALLOC(ncpus);
+
+	if (!set)
+		return NULL;
+	if (setsize)
+		*setsize = CPU_ALLOC_SIZE(ncpus);
+	if (nbits)
+		*nbits = cpuset_nbits(CPU_ALLOC_SIZE(ncpus));
+	return set;
+}
+
+void cpuset_free(cpu_set_t *set)
+{
+	CPU_FREE(set);
+}
+
+#if !HAVE_DECL_CPU_ALLOC
+/* Please, use CPU_COUNT_S() macro. This is fallback */
+int __cpuset_count_s(size_t setsize, const cpu_set_t *set)
+{
+	int s = 0;
+	const __cpu_mask *p = set->__bits;
+	const __cpu_mask *end = &set->__bits[setsize / sizeof (__cpu_mask)];
+
+	while (p < end) {
+		__cpu_mask l = *p++;
+
+		if (l == 0)
+			continue;
+# if LONG_BIT > 32
+		l = (l & 0x5555555555555555ul) + ((l >> 1) & 0x5555555555555555ul);
+		l = (l & 0x3333333333333333ul) + ((l >> 2) & 0x3333333333333333ul);
+		l = (l & 0x0f0f0f0f0f0f0f0ful) + ((l >> 4) & 0x0f0f0f0f0f0f0f0ful);
+		l = (l & 0x00ff00ff00ff00fful) + ((l >> 8) & 0x00ff00ff00ff00fful);
+		l = (l & 0x0000ffff0000fffful) + ((l >> 16) & 0x0000ffff0000fffful);
+		l = (l & 0x00000000fffffffful) + ((l >> 32) & 0x00000000fffffffful);
+# else
+		l = (l & 0x55555555ul) + ((l >> 1) & 0x55555555ul);
+		l = (l & 0x33333333ul) + ((l >> 2) & 0x33333333ul);
+		l = (l & 0x0f0f0f0ful) + ((l >> 4) & 0x0f0f0f0ful);
+		l = (l & 0x00ff00fful) + ((l >> 8) & 0x00ff00fful);
+		l = (l & 0x0000fffful) + ((l >> 16) & 0x0000fffful);
+# endif
+		s += l;
+	}
+	return s;
+}
+#endif
+
+/*
+ * Returns human readable representation of the cpuset. The output format is
+ * a list of CPUs with ranges (for example, "0,1,3-9").
+ */
+char *cpulist_create(char *str, size_t len,
+			cpu_set_t *set, size_t setsize)
+{
+	size_t i;
+	char *ptr = str;
+	int entry_made = 0;
+	size_t max = cpuset_nbits(setsize);
+
+	for (i = 0; i < max; i++) {
+		if (CPU_ISSET_S(i, setsize, set)) {
+			int rlen;
+			size_t j, run = 0;
+			entry_made = 1;
+			for (j = i + 1; j < max; j++) {
+				if (CPU_ISSET_S(j, setsize, set))
+					run++;
+				else
+					break;
+			}
+			if (!run)
+				rlen = snprintf(ptr, len, "%zd,", i);
+			else if (run == 1) {
+				rlen = snprintf(ptr, len, "%zd,%zd,", i, i + 1);
+				i++;
+			} else {
+				rlen = snprintf(ptr, len, "%zd-%zd,", i, i + run);
+				i += run;
+			}
+			if (rlen < 0 || (size_t) rlen + 1 > len)
+				return NULL;
+			ptr += rlen;
+			if (rlen > 0 && len > (size_t) rlen)
+				len -= rlen;
+			else
+				len = 0;
+		}
+	}
+	ptr -= entry_made;
+	*ptr = '\0';
+
+	return str;
+}
+
+/*
+ * Returns string with CPU mask.
+ */
+char *cpumask_create(char *str, size_t len,
+			cpu_set_t *set, size_t setsize)
+{
+	char *ptr = str;
+	char *ret = NULL;
+	int cpu;
+
+	for (cpu = cpuset_nbits(setsize) - 4; cpu >= 0; cpu -= 4) {
+		char val = 0;
+
+		if (len == (size_t) (ptr - str))
+			break;
+
+		if (CPU_ISSET_S(cpu, setsize, set))
+			val |= 1;
+		if (CPU_ISSET_S(cpu + 1, setsize, set))
+			val |= 2;
+		if (CPU_ISSET_S(cpu + 2, setsize, set))
+			val |= 4;
+		if (CPU_ISSET_S(cpu + 3, setsize, set))
+			val |= 8;
+
+		if (!ret && val)
+			ret = ptr;
+		*ptr++ = val_to_char(val);
+	}
+	*ptr = '\0';
+	return ret ? ret : ptr - 1;
+}
+
+/*
+ * Parses string with CPUs mask.
+ */
+int cpumask_parse(const char *str, cpu_set_t *set, size_t setsize)
+{
+	int len = strlen(str);
+	const char *ptr = str + len - 1;
+	int cpu = 0;
+
+	/* skip 0x, it's all hex anyway */
+	if (len > 1 && !memcmp(str, "0x", 2L))
+		str += 2;
+
+	CPU_ZERO_S(setsize, set);
+
+	while (ptr >= str) {
+		char val;
+
+		/* cpu masks in /sys uses comma as a separator */
+		if (*ptr == ',')
+			ptr--;
+
+		val = char_to_val(*ptr);
+		if (val == (char) -1)
+			return -1;
+		if (val & 1)
+			CPU_SET_S(cpu, setsize, set);
+		if (val & 2)
+			CPU_SET_S(cpu + 1, setsize, set);
+		if (val & 4)
+			CPU_SET_S(cpu + 2, setsize, set);
+		if (val & 8)
+			CPU_SET_S(cpu + 3, setsize, set);
+		len--;
+		ptr--;
+		cpu += 4;
+	}
+
+	return 0;
+}
+
+/*
+ * Parses string with list of CPU ranges.
+ * Returns 0 on success.
+ * Returns 1 on error.
+ * Returns 2 if fail is set and a cpu number passed in the list doesn't fit
+ * into the cpu_set. If fail is not set cpu numbers that do not fit are
+ * ignored and 0 is returned instead.
+ */
+int cpulist_parse(const char *str, cpu_set_t *set, size_t setsize, int fail)
+{
+	size_t max = cpuset_nbits(setsize);
+	const char *p, *q;
+	int r = 0;
+
+	q = str;
+	CPU_ZERO_S(setsize, set);
+
+	while (p = q, q = nexttoken(q, ','), p) {
+		unsigned int a;	/* beginning of range */
+		unsigned int b;	/* end of range */
+		unsigned int s;	/* stride */
+		const char *c1, *c2;
+		char c;
+
+		if ((r = sscanf(p, "%u%c", &a, &c)) < 1)
+			return 1;
+		b = a;
+		s = 1;
+
+		c1 = nexttoken(p, '-');
+		c2 = nexttoken(p, ',');
+		if (c1 != NULL && (c2 == NULL || c1 < c2)) {
+			if ((r = sscanf(c1, "%u%c", &b, &c)) < 1)
+				return 1;
+			c1 = nexttoken(c1, ':');
+			if (c1 != NULL && (c2 == NULL || c1 < c2)) {
+				if ((r = sscanf(c1, "%u%c", &s, &c)) < 1)
+					return 1;
+				if (s == 0)
+					return 1;
+			}
+		}
+
+		if (!(a <= b))
+			return 1;
+		while (a <= b) {
+			if (fail && (a >= max))
+				return 2;
+			CPU_SET_S(a, setsize, set);
+			a += s;
+		}
+	}
+
+	if (r == 2)
+		return 1;
+	return 0;
+}
+
+#ifdef TEST_PROGRAM
+
+#include <getopt.h>
+
+int main(int argc, char *argv[])
+{
+	cpu_set_t *set;
+	size_t setsize, buflen, nbits;
+	char *buf, *mask = NULL, *range = NULL;
+	int ncpus = 2048, rc, c;
+
+	static const struct option longopts[] = {
+	    { "ncpus", 1, 0, 'n' },
+	    { "mask",  1, 0, 'm' },
+	    { "range", 1, 0, 'r' },
+	    { NULL,    0, 0, 0 }
+	};
+
+	while ((c = getopt_long(argc, argv, "n:m:r:", longopts, NULL)) != -1) {
+		switch(c) {
+		case 'n':
+			ncpus = atoi(optarg);
+			break;
+		case 'm':
+			mask = strdup(optarg);
+			break;
+		case 'r':
+			range = strdup(optarg);
+			break;
+		default:
+			goto usage_err;
+		}
+	}
+
+	if (!mask && !range)
+		goto usage_err;
+
+	set = cpuset_alloc(ncpus, &setsize, &nbits);
+	if (!set)
+		err(EXIT_FAILURE, "failed to allocate cpu set");
+
+	/*
+	fprintf(stderr, "ncpus: %d, cpuset bits: %zd, cpuset bytes: %zd\n",
+			ncpus, nbits, setsize);
+	*/
+
+	buflen = 7 * nbits;
+	buf = malloc(buflen);
+	if (!buf)
+		err(EXIT_FAILURE, "failed to allocate cpu set buffer");
+
+	if (mask)
+		rc = cpumask_parse(mask, set, setsize);
+	else
+		rc = cpulist_parse(range, set, setsize, 0);
+
+	if (rc)
+		errx(EXIT_FAILURE, "failed to parse string: %s", mask ? : range);
+
+	printf("%-15s = %15s ", mask ? : range,
+				cpumask_create(buf, buflen, set, setsize));
+	printf("[%s]\n", cpulist_create(buf, buflen, set, setsize));
+
+	free(buf);
+	free(range);
+	cpuset_free(set);
+
+	return EXIT_SUCCESS;
+
+usage_err:
+	fprintf(stderr,
+		"usage: %s [--ncpus <num>] --mask <mask> | --range <list>",
+		program_invocation_short_name);
+	exit(EXIT_FAILURE);
+}
+#endif