shithub: util

ref: 284d1ce2d7f4992ef2edee3eaaeeecca70e535cd
dir: /lsm303dlhc.c/

#include <u.h>
#include <libc.h>
#include <fcall.h>
#include <thread.h>
#include <9p.h>

#define DATA "%s/i2c.%x.data"
#define CTL "%s/i2c.%x.ctl"

char *i2cdir = "/dev/i2c1";

enum {
	AccelAddr=	0x19,
		CtrlReg1=		0x20,
		CtrlReg2=		0x21,
		CtrlReg3=		0x22,
		CtrlReg4=		0x23,
		CtrlReg5=		0x24,
		CtrlReg6=		0x25,
		FifoCtrlReg=	0x2E,
		StatusReg=		0x27,
		Int1Cfg=		0x30,
		Int2Cfg=		0x34,
			AccelXHigh=		0x29,
			AccelXLow=		0x28,
			AccelYHigh=		0x2B,
			AccelYLow=		0x2A,
			AccelZHigh=		0x2D,
			AccelZLow=		0x2C,
	MagnetAddr=	0x1e,
		CraReg=		0x00,
		CrbReg=		0x01,
		MrReg=		0x02,
		IraReg=		0x0A,
		IrbReg=		0x0B,
		IrcReg=		0x0C,
			MagnetXHigh=	0x03,
			MagnetXLow=		0x04,
			MagnetYHigh=	0x05,
			MagnetYLow=		0x06,
			MagnetZHigh=	0x07,
			MagnetZLow=		0x08,
			TempHigh=		0x31,
			TempLow=		0x32,
	Ok=		0x00,
	Err=	0xFF,
};

void
ewrite(char *fname, char *buf, int n)
{
	int fd;

	fd = open(fname, OWRITE);
	if (fd == -1)
		sysfatal("open: %r");

	if (write(fd, buf, n) != n)
		sysfatal("write: %r");

	close(fd);
}

void
eread(char *fname, char *reg, int r, char *buf, int n)
{
	int fd;

	fd = open(fname, ORDWR);
	if (fd == -1)
		sysfatal("open: %r");

	if (write(fd, reg, r) != r)
		sysfatal("write: %r");

	if (read(fd, buf, n) != n)
		sysfatal("read: %r");

	close(fd);
}

void
fsread(Req *r)
{
	char *data;
	char reg[1];
	char out[1];
	short x, y, z;

	switch ((intptr)(r->fid->file->aux)) {
	case 1:
		data = smprint(DATA, i2cdir, AccelAddr);
		if (data == nil)
			sysfatal("smprint: %r");

		reg[0] = AccelXHigh;
		eread(data, reg, 1, out, 1);
		x = out[0] << 8;
		reg[0] = AccelXLow;
		eread(data, reg, 1, out, 1);
		x |= out[0];

		reg[0] = AccelYHigh;
		eread(data, reg, 1, out, 1);
		y = out[0] << 8;
		reg[0] = AccelYLow;
		eread(data, reg, 1, out, 1);
		y |= out[0];

		reg[0] = AccelZHigh;
		eread(data, reg, 1, out, 1);
		z = out[0] << 8;
		reg[0] = AccelZLow;
		eread(data, reg, 1, out, 1);
		z |= out[0];

		free(data);

		data = smprint("%11d %11d %11d\n", x, y, z);
		if (data == nil)
			sysfatal("smprint: %r");

		readstr(r, data);
		respond(r, nil);

		free(data);
		break;
	case 2:
		data = smprint(DATA, i2cdir, MagnetAddr);
		if (data == nil)
			sysfatal("smprint: %r");

		reg[0] = MagnetXHigh;
		eread(data, reg, 1, out, 1);
		x = out[0] << 8;
		reg[0] = MagnetXLow;
		eread(data, reg, 1, out, 1);
		x |= out[0];

		reg[0] = MagnetYHigh;
		eread(data, reg, 1, out, 1);
		y = out[0] << 8;
		reg[0] = MagnetYLow;
		eread(data, reg, 1, out, 1);
		y |= out[0];

		reg[0] = MagnetZHigh;
		eread(data, reg, 1, out, 1);
		z = out[0] << 8;
		reg[0] = MagnetZLow;
		eread(data, reg, 1, out, 1);
		z |= out[0];

		free(data);

		data = smprint("%11d %11d %11d\n", x, y, z);
		if (data == nil)
			sysfatal("smprint: %r");

		readstr(r, data);
		respond(r, nil);

		free(data);
		break;
	default:
		respond(r, "error");
		break;
	}
}

Srv fs = {
.read	= fsread,
};

void
init(void)
{
	char *data;
	char buf[2];

	data = smprint(DATA, i2cdir, AccelAddr);
	if (data == nil)
		sysfatal("smprint: %r");

	buf[0] = CtrlReg1;
	buf[1] = 0x27;
	ewrite(data, buf, 2);

	buf[0] = CtrlReg4;
	buf[1] = 0x40;
	ewrite(data, buf, 2);

	buf[0] = CtrlReg5;
	buf[1] = 0x40;
	ewrite(data, buf, 2);

	buf[0] = FifoCtrlReg;
	buf[1] = 0x80;
	ewrite(data, buf, 2);

	free(data);

	data = smprint(DATA, i2cdir, MagnetAddr);
	if (data == nil)
		sysfatal("smprint: %r");

	buf[0] = CraReg;
	buf[1] = 0x9C;
	ewrite(data, buf, 2);

	buf[0] = CrbReg;
	buf[1] = 0x20;
	ewrite(data, buf, 2);

	buf[0] = MrReg;
	buf[1] = 0x00;
	ewrite(data, buf, 2);

	buf[0] = IraReg;
	buf[1] = 0x48;
	ewrite(data, buf, 2);

	buf[0] = IrbReg;
	buf[1] = 0x34;
	ewrite(data, buf, 2);

	buf[0] = IrcReg;
	buf[1] = 0x33;
	ewrite(data, buf, 2);

	free(data);
}

void
main()
{
	init();

	fs.tree = alloctree("lsm303dlhc", "lsm303dlhc", DMDIR|0555, nil);
	createfile(fs.tree->root, "accel", "lsm303dlhc", 0444, (void*)1);
	createfile(fs.tree->root, "compass", "lsm303dlhc", 0444, (void*)2);
	postmountsrv(&fs, "lsm303dlhc", "/dev", MBEFORE);
}