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 }