Monday, December 30, 2013

Best method for setting & reading a hardware register

#include <stdio.h>


char *basecalc(unsigned int num, unsigned int base)
{
        static char buff[32];
        char *s = &buff[31];
        *s = '\0';

        while(num) {
                *(--s) = "0123456789ABCDEF"[num%base];
                num /= base;
        }
        return s;
}

void setreg(unsigned int *reg, unsigned int nofbits, unsigned int bitpos, unsigned int value)
{
        unsigned int mask;
        mask  = (~(~0 << nofbits) << bitpos);
        value = value << bitpos;
        *reg  = (*reg & ~mask) | (value & mask);
}

unsigned int getreg(unsigned int *reg, unsigned int nofbits, unsigned int bitpos)
{
        return (*reg >> bitpos) & (( 1 << nofbits) - 1) ;
}

int main()
{
        int num, nofbits, bitpos, value;
        printf("Enter the num: ");
        scanf("%d", &num);
        printf("Enter the nofbits: ");
        scanf("%d", &nofbits);
        printf("Enter the bitpos: ");
        scanf("%d", &bitpos);
        printf("Enter the value: ");
        scanf("%d", &value);

        printf("Read reg: %s\n", basecalc(getreg(&num, nofbits, bitpos), 2));

        printf("The num: %s\n", basecalc(num, 2));
        setreg(&num, nofbits, bitpos, value);
        printf("The result: %s\n", basecalc(num, 2));
        return 0;

}

Linux style
static void setreg(void __iomem *reg, u32 nofbits, u32 bitpos, u32 value)
{
        u32 mask;
        mask = (~(~0 << nofbits) << bitpos);
        value <<= bitpos;
        iowrite32(((ioread32(reg) & ~mask) | (value & mask)), reg);
}

static u32 getreg(void __iomem *reg, u32 nofbits, u32 bitpos)
{
        return (ioread32(reg) >> bitpos) & (~(~0 << nofbits));
}


Test 1
Enter the num: 25
Enter the nofbits: 2
Enter the bitpos: 3
Enter the value: 1

Read reg: 11
The num: 11001
The result: 1001

Test 2
Enter the num: 153
Enter the nofbits: 2
Enter the bitpos: 3
Enter the value: 2
The num: 10011001
The result: 10010001

No comments: