Imported Upstream version 1.3.2
[osspd.git] / ossp-util.c
1 /*
2  * ossp-util - OSS Proxy: Common utilities
3  *
4  * Copyright (C) 2008-2010  SUSE Linux Products GmbH
5  * Copyright (C) 2008-2010  Tejun Heo <tj@kernel.org>
6  *
7  * This file is released under the GPLv2.
8  */
9
10 #include <ctype.h>
11 #include <fcntl.h>
12 #include <inttypes.h>
13 #include <limits.h>
14 #include <stdarg.h>
15 #include <stdio.h>
16 #include <stdlib.h>
17 #include <sys/time.h>
18 #include <syslog.h>
19 #include <unistd.h>
20 #include "ossp-util.h"
21
22 #define BIT(nr)                 (1UL << (nr))
23 #define BIT_MASK(nr)            (1UL << ((nr) % BITS_PER_LONG))
24 #define BIT_WORD(nr)            ((nr) / BITS_PER_LONG)
25 #define BITS_TO_LONGS(nr)       DIV_ROUND_UP(nr, BITS_PER_BYTE * sizeof(long))
26 #define BITOP_WORD(nr)          ((nr) / BITS_PER_LONG)
27
28 char ossp_log_name[OSSP_LOG_NAME_LEN];
29 int ossp_log_level = OSSP_LOG_DFL;
30 int ossp_log_timestamp;
31
32 static const char *severity_strs[] = {
33         [OSSP_LOG_CRIT]         = "CRIT",
34         [OSSP_LOG_ERR]          = " ERR",
35         [OSSP_LOG_WARN]         = "WARN",
36         [OSSP_LOG_INFO]         = NULL,
37         [OSSP_LOG_DBG0]         = "DBG0",
38         [OSSP_LOG_DBG1]         = "DBG1",
39 };
40
41 static int severity_map[] = {
42         [OSSP_LOG_CRIT]         = LOG_ERR,
43         [OSSP_LOG_ERR]          = LOG_ERR,
44         [OSSP_LOG_WARN]         = LOG_WARNING,
45         [OSSP_LOG_INFO]         = LOG_INFO,
46         [OSSP_LOG_DBG0]         = LOG_DEBUG,
47         [OSSP_LOG_DBG1]         = LOG_DEBUG,
48 };
49
50 void log_msg(int severity, const char *fmt, ...)
51 {
52         static int syslog_opened = 0;
53         char buf[1024];
54         size_t len = sizeof(buf), off = 0;
55         va_list ap;
56
57         if (severity > abs(ossp_log_level))
58                 return;
59
60         if (ossp_log_level < 0 && !syslog_opened)
61                 openlog(ossp_log_name, 0, LOG_DAEMON);
62
63         assert(severity >= 0 && severity < ARRAY_SIZE(severity_strs));
64
65         if (ossp_log_timestamp) {
66                 static uint64_t start;
67                 uint64_t now;
68                 struct timeval tv;
69                 gettimeofday(&tv, NULL);
70                 now = tv.tv_sec * 1000 + tv.tv_usec / 1000;
71                 if (!start)
72                         start = now;
73
74                 off += snprintf(buf + off, len - off, "<%08"PRIu64"> ",
75                                 now - start);
76         }
77
78         if (ossp_log_level > 0) {
79                 char sev_buf[16] = "";
80                 if (severity_strs[severity])
81                         snprintf(sev_buf, sizeof(sev_buf), " %s",
82                                  severity_strs[severity]);
83                 off += snprintf(buf + off, len - off, "%s%s: ",
84                                 ossp_log_name, sev_buf);
85         } else if (severity_strs[severity])
86                 off += snprintf(buf + off, len - off, "%s ",
87                                 severity_strs[severity]);
88
89         va_start(ap, fmt);
90         off += vsnprintf(buf + off, len - off, fmt, ap);
91         va_end(ap);
92
93         off += snprintf(buf + off, len - off, "\n");
94
95         if (ossp_log_level > 0)
96                 fputs(buf, stderr);
97         else
98                 syslog(severity_map[severity], "%s", buf);
99 }
100
101 int read_fill(int fd, void *buf, size_t size)
102 {
103         while (size) {
104                 ssize_t ret;
105                 int rc;
106
107                 ret = read(fd, buf, size);
108                 if (ret <= 0) {
109                         if (ret == 0)
110                                 rc = -EIO;
111                         else
112                                 rc = -errno;
113                         err_e(rc, "failed to read_fill %zu bytes from fd %d",
114                               size, fd);
115                         return rc;
116                 }
117                 buf += ret;
118                 size -= ret;
119         }
120         return 0;
121 }
122
123 int write_fill(int fd, const void *buf, size_t size)
124 {
125         while (size) {
126                 ssize_t ret;
127                 int rc;
128
129                 ret = write(fd, buf, size);
130                 if (ret <= 0) {
131                         if (ret == 0)
132                                 rc = -EIO;
133                         else
134                                 rc = -errno;
135                         err_e(rc, "failed to write_fill %zu bytes to fd %d",
136                               size, fd);
137                         return rc;
138                 }
139                 buf += ret;
140                 size -= ret;
141         }
142         return 0;
143 }
144
145 void ring_fill(struct ring_buf *ring, const void *buf, size_t size)
146 {
147         size_t tail;
148
149         assert(ring_space(ring) >= size);
150
151         tail = (ring->head + ring->size - ring->bytes) % ring->size;
152
153         if (ring->head >= tail) {
154                 size_t todo = min(size, ring->size - ring->head);
155
156                 memcpy(ring->buf + ring->head, buf, todo);
157                 ring->head = (ring->head + todo) % ring->size;
158                 ring->bytes += todo;
159                 buf += todo;
160                 size -= todo;
161         }
162
163         assert(ring->size - ring->head >= size);
164         memcpy(ring->buf + ring->head, buf, size);
165         ring->head += size;
166         ring->bytes += size;
167 }
168
169 void *ring_data(struct ring_buf *ring, size_t *sizep)
170 {
171         size_t tail;
172
173         if (!ring->bytes)
174                 return NULL;
175
176         tail = (ring->head + ring->size - ring->bytes) % ring->size;
177
178         *sizep = min(ring->bytes, ring->size - tail);
179         return ring->buf + tail;
180 }
181
182 int ring_resize(struct ring_buf *ring, size_t new_size)
183 {
184         struct ring_buf new_ring = { .size = new_size };
185         void *p;
186         size_t size;
187
188         if (ring_bytes(ring) > new_size)
189                 return -ENOSPC;
190
191         new_ring.buf = calloc(1, new_size);
192         if (new_size && !new_ring.buf)
193                 return -ENOMEM;
194
195         while ((p = ring_data(ring, &size))) {
196                 ring_fill(&new_ring, p, size);
197                 ring_consume(ring, size);
198         }
199
200         free(ring->buf);
201         *ring = new_ring;
202         return 0;
203 }
204
205 int ensure_sbuf_size(struct sized_buf *sbuf, size_t size)
206 {
207         char *new_buf;
208
209         if (sbuf->size >= size)
210                 return 0;
211
212         new_buf = realloc(sbuf->buf, size);
213         if (size && !new_buf)
214                 return -ENOMEM;
215
216         sbuf->buf = new_buf;
217         sbuf->size = size;
218         return 0;
219 }
220
221 static unsigned long __ffs(unsigned long word)
222 {
223         int num = 0;
224
225         if (BITS_PER_LONG == 64) {
226                 if ((word & 0xffffffff) == 0) {
227                         num += 32;
228                         word >>= 32;
229                 }
230         }
231
232         if ((word & 0xffff) == 0) {
233                 num += 16;
234                 word >>= 16;
235         }
236         if ((word & 0xff) == 0) {
237                 num += 8;
238                 word >>= 8;
239         }
240         if ((word & 0xf) == 0) {
241                 num += 4;
242                 word >>= 4;
243         }
244         if ((word & 0x3) == 0) {
245                 num += 2;
246                 word >>= 2;
247         }
248         if ((word & 0x1) == 0)
249                 num += 1;
250         return num;
251 }
252
253 #define ffz(x)  __ffs(~(x))
254
255 unsigned long find_next_zero_bit(const unsigned long *addr, unsigned long size,
256                                  unsigned long offset)
257 {
258         const unsigned long *p = addr + BITOP_WORD(offset);
259         unsigned long result = offset & ~(BITS_PER_LONG-1);
260         unsigned long tmp;
261
262         if (offset >= size)
263                 return size;
264         size -= result;
265         offset %= BITS_PER_LONG;
266         if (offset) {
267                 tmp = *(p++);
268                 tmp |= ~0UL >> (BITS_PER_LONG - offset);
269                 if (size < BITS_PER_LONG)
270                         goto found_first;
271                 if (~tmp)
272                         goto found_middle;
273                 size -= BITS_PER_LONG;
274                 result += BITS_PER_LONG;
275         }
276         while (size & ~(BITS_PER_LONG-1)) {
277                 if (~(tmp = *(p++)))
278                         goto found_middle;
279                 result += BITS_PER_LONG;
280                 size -= BITS_PER_LONG;
281         }
282         if (!size)
283                 return result;
284         tmp = *p;
285
286 found_first:
287         tmp |= ~0UL << size;
288         if (tmp == ~0UL)        /* Are any bits zero? */
289                 return result + size;   /* Nope. */
290 found_middle:
291         return result + ffz(tmp);
292 }
293
294 void __set_bit(int nr, volatile unsigned long *addr)
295 {
296         unsigned long mask = BIT_MASK(nr);
297         unsigned long *p = ((unsigned long *)addr) + BIT_WORD(nr);
298
299         *p  |= mask;
300 }
301
302 void __clear_bit(int nr, volatile unsigned long *addr)
303 {
304         unsigned long mask = BIT_MASK(nr);
305         unsigned long *p = ((unsigned long *)addr) + BIT_WORD(nr);
306
307         *p &= ~mask;
308 }
309
310 int get_proc_self_info(pid_t pid, pid_t *ppid_r,
311                        char *cmd_buf, size_t cmd_buf_sz)
312
313 {
314         char path[64], buf[4096];
315         int fd = -1;
316         char *cmd_start, *cmd_end, *ppid_start, *end;
317         ssize_t ret;
318         pid_t ppid;
319         int i, rc;
320
321         snprintf(path, sizeof(path), "/proc/%ld/stat", (long)pid);
322         fd = open(path, O_RDONLY);
323         if (fd < 0) {
324                 rc = -errno;
325                 goto out;
326         }
327
328         ret = read(fd, buf, sizeof(buf));
329         if (ret < 0)
330                 goto out;
331         if (ret == sizeof(buf)) {
332                 rc = -EOVERFLOW;
333                 goto out;
334         }
335         buf[ret] = '\0';
336
337         rc = -EINVAL;
338         cmd_start = strchr(buf, '(');
339         cmd_end = strrchr(buf, ')');
340         if (!cmd_start || !cmd_end)
341                 goto out;
342         cmd_start++;
343
344         ppid_start = cmd_end;
345         for (i = 0; i < 3; i++) {
346                 ppid_start = strchr(ppid_start, ' ');
347                 if (!ppid_start)
348                         goto out;
349                 ppid_start++;
350         }
351
352         ppid = strtoul(ppid_start, &end, 10);
353         if (end == ppid_start || *end != ' ')
354                 goto out;
355
356         if (ppid_r)
357                 *ppid_r = ppid;
358         if (cmd_buf) {
359                 size_t len = min_t(size_t, cmd_end - cmd_start, cmd_buf_sz - 1);
360                 memcpy(cmd_buf, cmd_start, len);
361                 cmd_buf[len] = '\0';
362         }
363
364         rc = 0;
365  out:
366         close(fd);
367
368         return rc;
369 }