rom/filesys/SFS/FS/asmsupport.c

   1 /*
   2  * These functions implement bitfield and bitmap operations for any possible CPU
   3  * Used instead of m68k asm functions in asmsupport.s file.
   4  */
   5
   6 #include <exec/types.h>
   7
   8 #ifdef __AROS__
   9 #include <aros/macros.h>
  10 #include <aros/asmcall.h>
  11 #else
  12 #define AROS_BE2LONG(x) x
  13 #define AROS_LONG2BE(x) x
  14 #endif
  15 #include <stdio.h>
  16
  17 #include "asmsupport.h"
  18 #include "debug.h"
  19
  20 #if defined __i386__
  21 static inline LONG frs(ULONG mask)
  22 {
  23     LONG bit = 0;
  24
  25     if (mask)
  26         asm volatile("bsf %1, %0":"=r"(bit):"rm"(mask));
  27
  28     return (32-bit);
  29 }
  30
  31 #else
  32 static inline LONG frs(ULONG mask)
  33 {
  34     LONG bit;
  35
  36     if (mask == 0)
  37         return (31);
  38
  39     for (bit = 32; (mask & 1) != 1; bit--)
  40         mask = mask >> 1;
  41
  42     return (bit);
  43 }
  44 #endif
  45
  46 /* Finds last set bit in /data/ starting at /bitoffset/.  This function
  47    considers the MSB to be the first bit. */
  48 static inline WORD bfflo(ULONG data, WORD bitoffset)
  49 {
  50     ULONG mask = 0xffffffff << (31-bitoffset);
  51     data &= mask;
  52     return data == 0 ? -1 : frs(data)-1;
  53 }
  54
  55 /* Finds last zero bit in /data/ starting at /bitoffset/.  This function
  56    considers the MSB to be the first bit. */
  57 static inline WORD bfflz(ULONG data, WORD bitoffset)
  58 {
  59     return bfflo(~data, bitoffset);
  60 }
  61
  62 /* Sets /bits/ bits starting from /bitoffset/ in /data/.
  63    /bits/ must be between 1 and 32. */
  64 static inline ULONG bfset(ULONG data, WORD bitoffset, WORD bits)
  65 {
  66     ULONG mask = ~((1 << (32 - bits)) - 1);
  67     mask >>= bitoffset;
  68     return data | mask;
  69 }
  70
  71 /* Clears /bits/ bits starting from /bitoffset/ in /data/.
  72    /bits/ must be between 1 and 32. */
  73 static inline ULONG bfclr(ULONG data, WORD bitoffset, WORD bits)
  74 {
  75     ULONG mask = ~((1 << (32 - bits)) - 1);
  76     mask >>= bitoffset;
  77     return data & ~mask;
  78 }
  79
  80 ULONG bfcnto(ULONG v)
  81 {
  82     ULONG const w = v - ((v >> 1) & 0x55555555);                      // temp
  83     ULONG const x = (w & 0x33333333) + ((w >> 2) & 0x33333333);       // temp
  84     ULONG const c = (((x + (x >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24; // count
  85
  86     return c;
  87 }
  88
  89 ULONG bfcntz(ULONG v)
  90 {
  91     return bfcnto(~v);
  92 }
  93
  94 LONG bmflo(ULONG *bitmap, LONG bitoffset)
  95 {
  96     ULONG *scan = bitmap;
  97     LONG longs;
  98     int longoffset, bit;
  99
 100     longoffset = (bitoffset) >> 5;
 101     longs = longoffset;
 102     scan += longoffset;
 103
 104     bitoffset = bitoffset & 0x1F;
 105
 106     if (bitoffset != 0) {
 107         if ((bit = bfflo(AROS_BE2LONG(*scan), bitoffset)) >= 0) {
 108             return (bit + ((scan - bitmap) << 5));
 109         }
 110         scan--;
 111         longs--;
 112     }
 113
 114     while (longs-- >= 0) {
 115         if (*scan-- != 0) {
 116             scan++;
 117             return (bfflo(AROS_BE2LONG(*scan),31) + ((scan - bitmap) << 5));
 118         }
 119     }
 120
 121     return (-1);
 122 }
 123
 124 LONG bmflz(ULONG *bitmap, LONG bitoffset)
 125 {
 126     ULONG *scan = bitmap;
 127     LONG longs;
 128     int longoffset, bit;
 129
 130     longoffset = (bitoffset) >> 5;
 131     longs = longoffset;
 132     scan += longoffset;
 133
 134     bitoffset = bitoffset & 0x1F;
 135
 136     if (bitoffset != 0) {
 137         if ((bit = bfflz(AROS_BE2LONG(*scan), bitoffset)) >= 0) {
 138             return (bit + ((scan - bitmap) << 5));
 139         }
 140         scan--;
 141         longs--;
 142     }
 143
 144     while (longs-- >= 0) {
 145         if (*scan-- != 0xFFFFFFFF) {
 146             scan++;
 147             return (bfflz(AROS_BE2LONG(*scan),31) + ((scan - bitmap) << 5));
 148         }
 149     }
 150
 151     return (-1);
 152 }
 153
 154 /* This function finds the first set bit in a region of memory starting
 155    with /bitoffset/.  The region of memory is /longs/ longs long.  It
 156    returns the bitoffset of the first set bit it finds. */
 157
 158 LONG bmffo(ULONG *bitmap, LONG longs, LONG bitoffset)
 159 {
 160     ULONG *scan = bitmap;
 161     ULONG err = 32*longs;
 162     int longoffset, bit;
 163
 164     longoffset = bitoffset >> 5;
 165     longs -= longoffset;
 166     scan += longoffset;
 167
 168     bitoffset = bitoffset & 0x1F;
 169
 170     if (bitoffset != 0) {
 171         if ((bit = bfffo(AROS_BE2LONG(*scan), bitoffset)) < 32) {
 172             return (bit + ((scan - bitmap) << 5));
 173         }
 174         scan++;
 175         longs--;
 176     }
 177
 178     while (longs-- > 0) {
 179         if (*scan++ != 0) {
 180             --scan;
 181             return (bfffo(AROS_BE2LONG(*scan),0) + ((scan - bitmap) << 5));
 182         }
 183     }
 184
 185     return (err);
 186 }
 187
 188 /* This function finds the first unset bit in a region of memory starting
 189    with /bitoffset/.  The region of memory is /longs/ longs long.  It
 190    returns the bitoffset of the first unset bit it finds. */
 191
 192 LONG bmffz(ULONG *bitmap, LONG longs, LONG bitoffset)
 193 {
 194     ULONG *scan = bitmap;
 195     ULONG err = 32*longs;
 196     int longoffset, bit;
 197
 198     longoffset = bitoffset >> 5;
 199     longs -= longoffset;
 200     scan += longoffset;
 201
 202     bitoffset = bitoffset & 0x1F;
 203
 204     if (bitoffset != 0) {
 205         if ((bit = bfffz(AROS_BE2LONG(*scan), bitoffset)) < 32) {
 206             return (bit + ((scan - bitmap) << 5));
 207         }
 208         scan++;
 209         longs--;
 210     }
 211
 212     while (longs-- > 0) {
 213         if (*scan++ != 0xFFFFFFFF) {
 214             --scan;
 215             return (bfffz(AROS_BE2LONG(*scan),0) + ((scan - bitmap) << 5));
 216         }
 217     }
 218
 219     return (err);
 220 }
 221
 222 /* This function clears /bits/ bits in a region of memory starting
 223    with /bitoffset/.  The region of memory is /longs/ longs long.  If
 224    the region of memory is too small to clear /bits/ bits then this
 225    function exits after having cleared all bits till the end of the
 226    memory region.  In any case it returns the number of bits which
 227    were actually cleared. */
 228
 229 LONG bmclr(ULONG *bitmap, LONG longs, LONG bitoffset, LONG bits)
 230 {
 231     ULONG *scan = bitmap;
 232     int longoffset;
 233     int orgbits = bits;
 234
 235     longoffset = bitoffset >> 5;
 236     longs -= longoffset;
 237     scan += longoffset;
 238
 239     bitoffset = bitoffset & 0x1F;
 240
 241     if (bitoffset != 0) {
 242         if (bits < 32) {
 243             *scan = AROS_LONG2BE(bfclr(AROS_BE2LONG(*scan), bitoffset, bits));
 244         } else {
 245             *scan = AROS_LONG2BE(bfclr(AROS_BE2LONG(*scan), bitoffset, 32));
 246         }
 247         scan++;
 248         longs--;
 249         bits -= 32 - bitoffset;
 250     }
 251
 252     while (bits > 0 && longs-- > 0) {
 253         if (bits > 31) {
 254             *scan++ = 0;
 255         } else {
 256             *scan = AROS_LONG2BE(bfclr(AROS_BE2LONG(*scan), 0, bits));
 257         }
 258         bits -= 32;
 259     }
 260
 261     if (bits <= 0) {
 262         return (orgbits);
 263     }
 264     return (orgbits - bits);
 265 }
 266
 267 /* This function sets /bits/ bits in a region of memory starting
 268    with /bitoffset/.  The region of memory is /longs/ longs long.  If
 269    the region of memory is too small to set /bits/ bits then this
 270    function exits after having set all bits till the end of the
 271    memory region.  In any case it returns the number of bits which
 272    were actually set. */
 273
 274 LONG bmset(ULONG *bitmap, LONG longs, LONG bitoffset, LONG bits)
 275 {
 276     ULONG *scan = bitmap;
 277     int longoffset;
 278     int orgbits = bits;
 279
 280     longoffset = bitoffset >> 5;
 281     longs -= longoffset;
 282     scan += longoffset;
 283
 284     bitoffset = bitoffset & 0x1F;
 285
 286     if (bitoffset != 0) {
 287         if (bits < 32) {
 288             *scan = AROS_LONG2BE(bfset(AROS_BE2LONG(*scan), bitoffset, bits));
 289         } else {
 290             *scan = AROS_LONG2BE(bfset(AROS_BE2LONG(*scan), bitoffset, 32));
 291         }
 292         scan++;
 293         longs--;
 294         bits -= 32 - bitoffset;
 295     }
 296
 297     while (bits > 0 && longs-- > 0) {
 298         if (bits > 31) {
 299             *scan++ = 0xFFFFFFFF;
 300         } else {
 301             *scan = AROS_LONG2BE(bfset(AROS_BE2LONG(*scan), 0, bits));
 302         }
 303         bits -= 32;
 304     }
 305
 306     if (bits <= 0) {
 307         return (orgbits);
 308     }
 309     return (orgbits - bits);
 310 }
 311
 312 BOOL bmtsto(ULONG *bitmap, LONG bitoffset, LONG bits)
 313 {
 314     LONG longoffset = bitoffset >> 5;
 315     ULONG *scan = bitmap;
 316     ULONG mask;
 317
 318     scan += longoffset;
 319     bitoffset &= 0x1f;
 320
 321     if (bitoffset != 0)
 322     {
 323         if ((bits + bitoffset) < 32)
 324         {
 325             mask = (0xffffffff >> bitoffset) & (0xffffffff << (32 - (bits+bitoffset)));
 326             bits=0;
 327         }
 328         else
 329         {
 330             mask = (0xffffffff >> bitoffset);
 331             bits -= (32-bitoffset);
 332         }
 333
 334         if ((mask & AROS_BE2LONG(*scan)) != mask)
 335             return FALSE;
 336         scan++;
 337     }
 338
 339     while (bits > 0)
 340     {
 341         if (bits >= 32)
 342         {
 343             mask=0xffffffff;
 344             bits -= 32;
 345         }
 346         else
 347         {
 348             mask = 0xffffffff << (32-bits);
 349             bits = 0;
 350         }
 351         if ((mask & AROS_BE2LONG(*scan)) != mask)
 352             return FALSE;
 353         scan++;
 354     }
 355
 356     return TRUE;
 357 }
 358
 359 BOOL bmtstz(ULONG *bitmap, LONG bitoffset, LONG bits)
 360 {
 361     LONG longoffset = bitoffset >> 5;
 362     ULONG *scan = bitmap;
 363     ULONG mask;
 364
 365     scan += longoffset;
 366     bitoffset &= 0x1f;
 367
 368     if (bitoffset != 0)
 369     {
 370         if ((bits + bitoffset) < 32)
 371         {
 372             mask = (0xffffffff >> bitoffset) & (0xffffffff << (32 - (bits+bitoffset)));
 373             bits=0;
 374         }
 375         else
 376         {
 377             mask = (0xffffffff >> bitoffset);
 378             bits -= (32-bitoffset);
 379         }
 380         if ((mask & AROS_BE2LONG(*scan)) != 0)
 381             return FALSE;
 382         scan++;
 383     }
 384
 385     while (bits > 0)
 386     {
 387         if (bits >= 32)
 388         {
 389             mask=0xffffffff;
 390             bits -= 32;
 391         }
 392         else
 393         {
 394             mask = 0xffffffff << (32-bits);
 395             bits = 0;
 396         }
 397         if ((mask & AROS_BE2LONG(*scan)) != 0)
 398             return FALSE;
 399         scan++;
 400     }
 401
 402     return TRUE;
 403 }
 404
 405 ULONG bmcnto(ULONG *bitmap, LONG bitoffset, LONG bits)
 406 {
 407     LONG longoffset = bitoffset >> 5;
 408     ULONG *scan = bitmap;
 409     ULONG count = 0;
 410     ULONG mask;
 411
 412     scan += longoffset;
 413     bitoffset &= 0x1f;
 414
 415     if (bitoffset != 0)
 416     {
 417         if ((bits + bitoffset) < 32)
 418         {
 419             mask = (0xffffffff >> bitoffset) & (0xffffffff << (32 - (bits+bitoffset)));
 420             bits=0;
 421         }
 422         else
 423         {
 424             mask = (0xffffffff >> bitoffset);
 425             bits -= (32-bitoffset);
 426         }
 427         count += bfcnto(AROS_BE2LONG(*scan) & mask);
 428         scan++;
 429     }
 430
 431     while (bits > 0)
 432     {
 433         if (bits >= 32)
 434         {
 435             mask=0xffffffff;
 436             bits -= 32;
 437         }
 438         else
 439         {
 440             mask = 0xffffffff << (32-bits);
 441             bits = 0;
 442         }
 443         count += bfcnto(AROS_BE2LONG(*scan) & mask);
 444         scan++;
 445     }
 446
 447     return count;
 448 }
 449
 450 ULONG bmcntz(ULONG *bitmap, LONG bitoffset, LONG bits)
 451 {
 452     LONG longoffset = bitoffset >> 5;
 453     ULONG *scan = bitmap;
 454     ULONG count = 0;
 455     ULONG mask;
 456
 457     scan += longoffset;
 458     bitoffset &= 0x1f;
 459
 460     if (bitoffset != 0)
 461     {
 462         if ((bits + bitoffset) < 32)
 463         {
 464             mask = ~((0xffffffff >> bitoffset) & (0xffffffff << (32 - (bits+bitoffset))));
 465             bits=0;
 466         }
 467         else
 468         {
 469             mask = ~(0xffffffff >> bitoffset);
 470             bits -= (32-bitoffset);
 471         }
 472
 473         count += bfcntz(AROS_BE2LONG(*scan) | mask);
 474         scan++;
 475     }
 476
 477     while (bits > 0)
 478     {
 479         if (bits >= 32)
 480         {
 481             mask=0;
 482             bits -= 32;
 483         }
 484         else
 485         {
 486             mask = ~(0xffffffff << (32-bits));
 487             bits = 0;
 488         }
 489
 490         count += bfcntz(AROS_BE2LONG(*scan) | mask);
 491         scan++;
 492     }
 493
 494     return count;
 495 }
 496
 497 ULONG CALCCHECKSUM(ULONG blocksize, ULONG *block)
 498 {
 499     ULONG sum=1;
 500     blocksize >>=4;
 501
 502     while(blocksize--)
 503     {
 504 #if 0
 505         sum += (*block++);
 506         sum += (*block++);
 507         sum += (*block++);
 508         sum += (*block++);
 509 #else
 510         sum += (ULONG)BE2L(*block++);
 511         sum += (ULONG)BE2L(*block++);
 512         sum += (ULONG)BE2L(*block++);
 513         sum += (ULONG)BE2L(*block++);
 514 #endif
 515     }
 516
 517     return sum;
 518 }