1/*-
2 * Copyright (c) 2021 The FreeBSD Foundation
3 *
4 * This software were developed by Konstantin Belousov <kib@FreeBSD.org>
5 * under sponsorship from the FreeBSD Foundation.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 * 1. Redistributions of source code must retain the above copyright
11 *    notice, this list of conditions and the following disclaimer.
12 * 2. Redistributions in binary form must reproduce the above copyright
13 *    notice, this list of conditions and the following disclaimer in the
14 *    documentation and/or other materials provided with the distribution.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
26 * SUCH DAMAGE.
27 */
28
29#define	_WANT_P_OSREL
30#include <sys/param.h>
31#include <sys/sysctl.h>
32#include <errno.h>
33#include <sched.h>
34#include <string.h>
35
36#include "libc_private.h"
37
38int
39sched_setaffinity(pid_t pid, size_t cpusetsz, const cpuset_t *cpuset)
40{
41	static int mp_maxid;
42	cpuwhich_t which;
43	cpuset_t c;
44	int error, lbs, cpu;
45	size_t len, sz;
46
47	if (__getosreldate() < P_OSREL_TIDPID) {
48		if (pid == 0 || pid > _PID_MAX)
49			which = CPU_WHICH_TID;
50		else
51			which = CPU_WHICH_PID;
52	} else
53		which = CPU_WHICH_TIDPID;
54
55	sz = cpusetsz > sizeof(cpuset_t) ? sizeof(cpuset_t) : cpusetsz;
56	memset(&c, 0, sizeof(c));
57	memcpy(&c, cpuset, sz);
58
59	/* Linux ignores high bits */
60	if (mp_maxid == 0) {
61		len = sizeof(mp_maxid);
62		error = sysctlbyname("kern.smp.maxid", &mp_maxid, &len,
63		    NULL, 0);
64		if (error == -1)
65			return (error);
66	}
67	lbs = CPU_FLS(&c) - 1;
68	if (lbs > mp_maxid) {
69		CPU_FOREACH_ISSET(cpu, &c)
70			if (cpu > mp_maxid)
71				CPU_CLR(cpu, &c);
72	}
73	error = cpuset_setaffinity(CPU_LEVEL_WHICH, which,
74	    pid == 0 ? -1 : pid, sizeof(cpuset_t), &c);
75	if (error == -1 && errno == EDEADLK)
76		errno = EINVAL;
77
78	return (error);
79}
80