src/main/drivers/sdcard/sdcard_standard.c

   1 #include <stdint.h>
   2
   3 #include "sdcard_standard.h"
   4
   5 #define MIN(a, b) ((a) < (b) ? (a) : (b))
   6
   7 /**
   8  * Read a bitfield from an array of bits (the bit at index 0 being the most-significant bit of the first byte in
   9  * the buffer).
  10  */
  11 uint32_t readBitfield(uint8_t *buffer, unsigned bitIndex, unsigned bitLen)
  12 {
  13     uint32_t result = 0;
  14     unsigned bitInByteOffset = bitIndex % 8;
  15     uint8_t bufferByte;
  16
  17     buffer += bitIndex / 8;
  18
  19     // Align the bitfield to be read to the top of the buffer
  20     bufferByte = *buffer << bitInByteOffset;
  21
  22     while (bitLen > 0) {
  23         unsigned bitsThisLoop = MIN(8 - bitInByteOffset, bitLen);
  24
  25         result = (result << bitsThisLoop) | (bufferByte >> (8 - bitsThisLoop));
  26
  27         buffer++;
  28         bufferByte = *buffer;
  29
  30         bitLen -= bitsThisLoop;
  31         bitInByteOffset = 0;
  32     }
  33
  34     return result;
  35 }