I've got 32-bit registers with field defined as bit-masks, e.g.

#define BM_TEST_FIELD 0x000F0000

I need a macro that allows me to set a field (defined by its bit-mask) of a register (defined by its address) to a given value. Here's what I came up with:

#include <stdio.h>
#include <assert.h>

typedef unsigned int u32;

/* 
 * Set a given field defined by a bit-mask MASK of a 32-bit register at address
 * ADDR to a value VALUE.
 */
#define SET_REGISTER_FIELD(ADDR, MASK, VALUE)                                      \
{                                                                                  \
  u32 mask=(MASK); u32 value=(VALUE);                                              \
  u32 mem_reg = *(volatile u32*)(ADDR); /* Get current register value           */ \
  assert((MASK) != 0);                  /* Null masks are not supported         */ \
  while(0 == (mask & 0x01))             /* Shift the value to the left until    */ \
  {                                     /* it aligns with the bit field         */ \
    mask = mask >> 1; value = value << 1;                                          \
  }                                                                                \
  mem_reg &= ~(MASK);                   /* Clear previous register field value  */ \
  mem_reg |= value;                     /* Update register field with new value */ \
  *(volatile u32*)(ADDR) = mem_reg;     /* Update actual register               */ \
}

/* Test case */
#define BM_TEST_FIELD 0x000F0000
int main()
{
  u32 reg = 0x12345678;
  printf("Register before: 0x%.8X\n", reg);/* should be 0x12345678 */
  SET_REGISTER_FIELD(&reg, BM_TEST_FIELD, 0xA);
  printf("Register after: 0x%.8X\n", reg); /* should be 0x123A5678 */
  return 0;
}

Is there a simpler way to do it?