> +     if (buflen >= 31)
> +             cnt = 31;
>       else
> -             return buflen;
> +             cnt = buflen;
> +

I could simplify this if/else by:

      cnt = buflen & 0x1F;

but I think it will not be readable for other people.