src/u16id.c

   1 /*
   2  * Copyright (C) 2015-2019 "IoT.bzh"
   3  * Author José Bollo <jose.bollo@iot.bzh>
   4  *
   5  * Licensed under the Apache License, Version 2.0 (the "License");
   6  * you may not use this file except in compliance with the License.
   7  * You may obtain a copy of the License at
   8  *
   9  *   http://www.apache.org/licenses/LICENSE-2.0
  10  *
  11  * Unless required by applicable law or agreed to in writing, software
  12  * distributed under the License is distributed on an "AS IS" BASIS,
  13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14  * See the License for the specific language governing permissions and
  15  * limitations under the License.
  16  */
  17
  18 #include <stdint.h>
  19 #include <malloc.h>
  20 #include <errno.h>
  21
  22 #include "u16id.h"
  23
  24 /* compute P, the count of bits of pointers */
  25 #if UINTPTR_MAX == (18446744073709551615UL)
  26 #  define P 64
  27 #elif UINTPTR_MAX == (4294967295U)
  28 #  define P 32
  29 #elif UINTPTR_MAX == (65535U)
  30 #  define P 16
  31 #else
  32 #  error "Unsupported pointer size"
  33 #endif
  34
  35 /* granule of allocation */
  36 #define N 4
  37
  38 /*
  39  * The u16id maps are made of a single block of memory structured
  40  * as an array of uint16_t followed by an array of void*. To ensure
  41  * that void* pointers are correctly aligned, the array of uint16_t
  42  * at head is a multiple of N items, with N being a multiple of 2
  43  * if void* is 32 bits or 4 if void* is 64 bits.
  44  *
  45  * The first item of the array of uint16_t is used to record the
  46  * upper index of valid uint16_t ids.
  47  *
  48  * +-----+-----+-----+-----+ - - - - - - - - +-----+-----+-----+-----+ - - - - - - - -
  49  * |upper| id1 | id2 | id3 |                 |         ptr1          |
  50  * +-----+-----+-----+-----+ - - - - - - - - +-----+-----+-----+-----+ - - - - - - - -
  51  */
  52
  53 static inline uint16_t get_capacity(uint16_t upper)
  54 {
  55         /* capacity is the smallest kN-1 such that kN-1 >= upper) */
  56 #if N == 2 || N == 4 || N == 8 || N == 16
  57         return upper | (N - 1);
  58 #else
  59 #       error "not supported"
  60 #endif
  61 }
  62
  63 typedef struct {
  64         uint16_t upper;
  65         uint16_t capacity;
  66         uint16_t *ids;
  67         void **ptrs;
  68 } flat_t;
  69
  70 static void flatofup(flat_t *flat, void *base, uint16_t up)
  71 {
  72         uint16_t cap, *ids;
  73
  74         flat->upper = up;
  75         flat->capacity = cap = get_capacity(up);
  76         flat->ids = ids = base;
  77         flat->ptrs = ((void**)(&ids[cap + 1])) - 1;
  78 }
  79
  80 static void flatof(flat_t *flat, void *base)
  81 {
  82         if (base)
  83                 flatofup(flat, base, *(uint16_t*)base);
  84         else {
  85                 flat->upper = flat->capacity = 0;
  86                 flat->ids = NULL;
  87                 flat->ptrs = NULL;
  88         }
  89 }
  90
  91 static inline size_t size(uint16_t capacity)
  92 {
  93         return sizeof(uint16_t) * (capacity + 1)
  94                 + sizeof(void*) * capacity;
  95 }
  96
  97 static inline uint16_t search(flat_t *flat, uint16_t id)
  98 {
  99         uint16_t *ids = flat->ids;
 100         uint16_t r = flat->upper;
 101         while(r && ids[r] != id)
 102                 r--;
 103         return r;
 104 }
 105
 106 static void *add(flat_t *flat, uint16_t id, void *ptr)
 107 {
 108         void *grown, *result;
 109         flat_t oflat;
 110         uint16_t nupper, oupper;
 111
 112         oupper = flat->upper;
 113         nupper = (uint16_t)(oupper + 1);
 114         result = flat->ids;
 115         if (nupper > flat->capacity) {
 116                 grown = realloc(result, size(get_capacity(nupper)));
 117                 if (grown == NULL)
 118                         return NULL;
 119                 result = grown;
 120                 flatofup(flat, grown, nupper);
 121                 if (oupper) {
 122                         flatofup(&oflat, grown, oupper);
 123                         while (oupper) {
 124                                 flat->ptrs[oupper] = oflat.ptrs[oupper];
 125                                 oupper--;
 126                         }
 127                 }
 128         }
 129         /* flat->upper = nupper; NOT DONE BECAUSE NOT NEEDED */
 130         flat->ids[0] = nupper;
 131         flat->ids[nupper] = id;
 132         flat->ptrs[nupper] = ptr;
 133         return result;
 134 }
 135
 136 static void *drop(flat_t *flat, uint16_t index)
 137 {
 138         void **ptrs, *result;
 139         uint16_t upper, idx, capa;
 140
 141         upper = flat->upper;
 142         if (index != upper) {
 143                 flat->ids[index] = flat->ids[upper];
 144                 flat->ptrs[index] = flat->ptrs[upper];
 145         }
 146         flat->ids[0] = --upper;
 147         capa = get_capacity(upper);
 148         result = flat->ids;
 149         if (capa != flat->capacity) {
 150                 ptrs = flat->ptrs;
 151                 flatofup(flat, result, upper);
 152                 idx = 1;
 153                 while(idx <= upper) {
 154                         flat->ptrs[idx] = ptrs[idx];
 155                         idx++;
 156                 }
 157 #if U16ID_ALWAYS_SHRINK
 158                 result = realloc(flat->ids, size(capa));
 159                 if (result == NULL)
 160                         result = flat->ids;
 161 #endif
 162         }
 163         return result;
 164 }
 165
 166 static void dropall(void **pbase)
 167 {
 168         void *base;
 169
 170         base = *pbase;
 171         if (base)
 172                 *(uint16_t*)base = 0;
 173 }
 174
 175 static void destroy(void **pbase)
 176 {
 177         void *base;
 178
 179         base = *pbase;
 180         *pbase = NULL;
 181         free(base);
 182 }
 183
 184 static int create(void **pbase)
 185 {
 186         void *base;
 187
 188         *pbase = base = malloc(size(get_capacity(0)));
 189         if (base == NULL)
 190                 return -1;
 191         *(uint16_t*)base = 0;
 192         return 0;
 193 }
 194
 195 /**********************************************************************/
 196 /**        u16id2ptr                                                 **/
 197 /**********************************************************************/
 198
 199 int u16id2ptr_create(struct u16id2ptr **pi2p)
 200 {
 201         return create((void**)pi2p);
 202 }
 203
 204 void u16id2ptr_destroy(struct u16id2ptr **pi2p)
 205 {
 206         destroy((void**)pi2p);
 207 }
 208
 209 void u16id2ptr_dropall(struct u16id2ptr **pi2p)
 210 {
 211         dropall((void**)pi2p);
 212 }
 213
 214 int u16id2ptr_has(struct u16id2ptr *i2p, uint16_t id)
 215 {
 216         flat_t flat;
 217
 218         flatof(&flat, i2p);
 219         return search(&flat, id) != 0;
 220 }
 221
 222 int u16id2ptr_add(struct u16id2ptr **pi2p, uint16_t id, void *ptr)
 223 {
 224         struct u16id2ptr *i2p;
 225         uint16_t index;
 226         flat_t flat;
 227
 228         i2p = *pi2p;
 229         flatof(&flat, i2p);
 230         index = search(&flat, id);
 231         if (index) {
 232                 errno = EEXIST;
 233                 return -1;
 234         }
 235         i2p = add(&flat, id, ptr);
 236         if (!i2p)
 237                 return -1;
 238         *pi2p = i2p;
 239         return 0;
 240 }
 241
 242 int u16id2ptr_set(struct u16id2ptr **pi2p, uint16_t id, void *ptr)
 243 {
 244         struct u16id2ptr *i2p;
 245         uint16_t index;
 246         flat_t flat;
 247
 248         i2p = *pi2p;
 249         flatof(&flat, i2p);
 250         index = search(&flat, id);
 251         if (index)
 252                 flat.ptrs[index] = ptr;
 253         else {
 254                 i2p = add(&flat, id, ptr);
 255                 if (!i2p)
 256                         return -1;
 257                 *pi2p = i2p;
 258         }
 259         return 0;
 260 }
 261
 262 int u16id2ptr_put(struct u16id2ptr *i2p, uint16_t id, void *ptr)
 263 {
 264         uint16_t index;
 265         flat_t flat;
 266
 267         flatof(&flat, i2p);
 268         index = search(&flat, id);
 269         if (index) {
 270                 flat.ptrs[index] = ptr;
 271                 return 0;
 272         }
 273         errno = ENOENT;
 274         return -1;
 275 }
 276
 277 int u16id2ptr_get(struct u16id2ptr *i2p, uint16_t id, void **pptr)
 278 {
 279         uint16_t index;
 280         flat_t flat;
 281
 282         flatof(&flat, i2p);
 283         index = search(&flat, id);
 284         if (index) {
 285                 *pptr = flat.ptrs[index];
 286                 return 0;
 287         }
 288         errno = ENOENT;
 289         return -1;
 290 }
 291
 292 int u16id2ptr_drop(struct u16id2ptr **pi2p, uint16_t id, void **pptr)
 293 {
 294         struct u16id2ptr *i2p;
 295         uint16_t index;
 296         flat_t flat;
 297
 298         i2p = *pi2p;
 299         flatof(&flat, i2p);
 300         index = search(&flat, id);
 301         if (!index) {
 302                 errno = ENOENT;
 303                 return -1;
 304         }
 305         if (pptr)
 306                 *pptr = flat.ptrs[index];
 307         i2p = drop(&flat, index);
 308         if (!i2p)
 309                 return -1;
 310         *pi2p = i2p;
 311         return 0;
 312 }
 313
 314 int u16id2ptr_count(struct u16id2ptr *i2p)
 315 {
 316         return i2p ? ((int)*(uint16_t*)i2p) : 0;
 317 }
 318
 319 int u16id2ptr_at(struct u16id2ptr *i2p, int index, uint16_t *pid, void **pptr)
 320 {
 321         flat_t flat;
 322
 323         flatof(&flat, i2p);
 324         if (index >= 0 && index < (int)flat.upper) {
 325                 *pid = flat.ids[index + 1];
 326                 *pptr = flat.ptrs[index + 1];
 327                 return 0;
 328         }
 329         errno = EINVAL;
 330         return -1;
 331 }
 332
 333 void u16id2ptr_forall(struct u16id2ptr *i2p, void (*callback)(void*closure, uint16_t id, void *ptr), void *closure)
 334 {
 335         flat_t flat;
 336
 337         flatof(&flat, i2p);
 338         while (flat.upper) {
 339                 callback(closure, flat.ids[flat.upper], flat.ptrs[flat.upper]);
 340                 flat.upper--;
 341         }
 342 }
 343
 344 /**********************************************************************/
 345 /**        u16id2bool                                                **/
 346 /**********************************************************************/
 347
 348 int u16id2bool_create(struct u16id2bool **pi2b)
 349 {
 350         return create((void**)pi2b);
 351 }
 352
 353 void u16id2bool_destroy(struct u16id2bool **pi2b)
 354 {
 355         destroy((void**)pi2b);
 356 }
 357
 358 void u16id2bool_clearall(struct u16id2bool **pi2b)
 359 {
 360         dropall((void**)pi2b);
 361 }
 362
 363 int u16id2bool_get(struct u16id2bool *i2b, uint16_t id)
 364 {
 365         uintptr_t mask, field;
 366         uint16_t index, idm;
 367         flat_t flat;
 368
 369         flatof(&flat, i2b);
 370         idm = (uint16_t)(id & ~(P - 1));
 371         index = search(&flat, idm);
 372         if (!index)
 373                 return 0;
 374
 375         field = (uintptr_t)flat.ptrs[index];
 376         mask = (uintptr_t)((uintptr_t)1 << (id & (P - 1)));
 377         return (field & mask) != 0;
 378 }
 379
 380 int u16id2bool_set(struct u16id2bool **pi2b, uint16_t id, int value)
 381 {
 382         struct u16id2bool *i2b;
 383         uintptr_t mask, field, ofield;
 384         uint16_t index, idm;
 385         flat_t flat;
 386
 387         i2b = *pi2b;
 388         flatof(&flat, i2b);
 389         idm = (uint16_t)(id & ~(P - 1));
 390         index = search(&flat, idm);
 391         ofield = index ? (uintptr_t)flat.ptrs[index] : 0;
 392         mask = (uintptr_t)((uintptr_t)1 << (id & (P - 1)));
 393         if (value)
 394                 field = ofield | mask;
 395         else
 396                 field = ofield & ~mask;
 397         if (field != ofield) {
 398                 if (field) {
 399                         if (index)
 400                                 flat.ptrs[index] = (void*)field;
 401                         else {
 402                                 i2b = add(&flat, idm, (void*)field);
 403                                 if (!i2b)
 404                                         return -1;
 405                                 *pi2b = i2b;
 406                         }
 407                 } else {
 408                         if (index) {
 409                                 i2b = drop(&flat, index);
 410                                 if (!i2b)
 411                                         return -1;
 412                                 *pi2b = i2b;
 413                         }
 414                 }
 415         }
 416         return (ofield & mask) != 0;
 417 }