diff --git a/src/rt-app.c b/src/rt-app.c
index 673df44..963ad57 100644
--- a/src/rt-app.c
+++ b/src/rt-app.c
@@ -51,6 +51,12 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  */
 #define FORKS_LIMIT		1024
 
+
+/*
+ * Maximum number of CPUs rt-app will be able to detect.
+ */
+#define MAX_CPUS		1024
+
 static volatile sig_atomic_t continue_running;
 static pthread_data_t *threads;
 static int nthreads;
@@ -733,6 +739,12 @@ static void __shutdown(bool force_terminate)
 	{
 		/* clean up tdata if this was a forked thread */
 		free(threads[i].data->name);
+		if (threads[i].data->def_cpu_data.cpuset)
+			CPU_FREE(threads[i].data->def_cpu_data.cpuset);
+
+		if (threads[i].data->def_cpu_data.cpuset_str)
+			free(threads[i].data->def_cpu_data.cpuset_str);
+
 		free(threads[i].data);
 	}
 
@@ -793,7 +805,7 @@ static int create_cpuset_str(cpuset_data_t *cpu_data)
 	for (i = 0; i < 10000 && cpu_count; ++i) {
 		unsigned int n;
 
-		if (CPU_ISSET(i, cpu_data->cpuset)) {
+		if (CPU_ISSET_S(cpu_data->cpusetsize, i, cpu_data->cpuset)) {
 			--cpu_count;
 			if (size_needed <= (idx + 1)) {
 				log_error("Not enough memory for array");
@@ -830,21 +842,20 @@ static void set_thread_affinity(thread_data_t *data, cpuset_data_t *cpu_data)
 
 	if (data->def_cpu_data.cpuset == NULL) {
 		/* Get default affinity */
-		cpu_set_t cpuset;
-		unsigned int cpu_count;
+		data->def_cpu_data.cpusetsize = CPU_ALLOC_SIZE(MAX_CPUS);
+		data->def_cpu_data.cpuset = CPU_ALLOC(MAX_CPUS);
+		if (!data->def_cpu_data.cpuset) {
+			perror("cpu_set_t malloc");
+			exit(EXIT_FAILURE);
+		}
 
 		ret = pthread_getaffinity_np(pthread_self(),
-						    sizeof(cpu_set_t), &cpuset);
+					     data->def_cpu_data.cpusetsize, data->def_cpu_data.cpuset);
 		if (ret != 0) {
 			errno = ret;
 			perror("pthread_get_affinity");
 			exit(EXIT_FAILURE);
 		}
-		cpu_count = CPU_COUNT(&cpuset);
-		data->def_cpu_data.cpusetsize = CPU_ALLOC_SIZE(cpu_count);
-		data->def_cpu_data.cpuset = CPU_ALLOC(cpu_count);
-		memcpy(data->def_cpu_data.cpuset, &cpuset,
-						data->def_cpu_data.cpusetsize);
 		create_cpuset_str(&data->def_cpu_data);
 		data->curr_cpu_data = &data->def_cpu_data;
 	}
@@ -861,8 +872,12 @@ static void set_thread_affinity(thread_data_t *data, cpuset_data_t *cpu_data)
 	if (actual_cpu_data->cpuset == NULL)
 		actual_cpu_data = &data->def_cpu_data;
 
-	if (!CPU_EQUAL(actual_cpu_data->cpuset, data->curr_cpu_data->cpuset))
-	{
+	if (
+	    actual_cpu_data->cpusetsize != data->curr_cpu_data->cpusetsize ||
+	    !CPU_EQUAL_S(
+			 actual_cpu_data->cpusetsize,
+			 actual_cpu_data->cpuset, data->curr_cpu_data->cpuset))
+	  {
 		log_debug("[%d] setting cpu affinity to CPU(s) %s", data->ind,
 			actual_cpu_data->cpuset_str);
 		ret = pthread_setaffinity_np(pthread_self(),
diff --git a/src/rt-app_parse_config.c b/src/rt-app_parse_config.c
index 3fc699b..a80c160 100644
--- a/src/rt-app_parse_config.c
+++ b/src/rt-app_parse_config.c
@@ -764,17 +764,17 @@ static void parse_cpuset_data(struct json_object *obj, cpuset_data_t *data)
 		data->cpuset_str = strdup(json_object_to_json_string(cpuset_obj));
 		data->cpusetsize = sizeof(cpu_set_t);
 		data->cpuset = malloc(data->cpusetsize);
-		CPU_ZERO(data->cpuset);
+		CPU_ZERO_S(data->cpusetsize, data->cpuset);
 		for (i = 0; i < json_object_array_length(cpuset_obj); i++) {
 			cpu = json_object_array_get_idx(cpuset_obj, i);
 			cpu_idx = json_object_get_int(cpu);
 			if (cpu_idx > max_cpu) {
 				log_critical(PIN2 "Invalid cpu %u in cpuset %s", cpu_idx, data->cpuset_str);
-				free(data->cpuset);
+				CPU_FREE(data->cpuset);
 				free(data->cpuset_str);
 				exit(EXIT_INV_CONFIG);
 			}
-			CPU_SET(cpu_idx, data->cpuset);
+			CPU_SET_S(cpu_idx, data->cpusetsize, data->cpuset);
 		}
 	} else {
 		data->cpuset_str = strdup("-");