EMC2 running on Raspberry Pi?

// copyright mungkie 2012 


/* MCP23017 pinouts

GPB0 			01[	U	]28 GPA7
GPB1			02[		]27 GPA6
GPB2			03[		]26 GPA5
GPB3			04[		]25 GPA4
GPB4			05[		]24 GPA3
GPB5			06[		]23 GPA2
GPB6			07[		]22 GPA1
GPB7			08[		]21 GPA0
Vdd			09[		]20 INTA
Vss			10[		]19 INTB
NC			11[		]18 /RESET
SCL			12[		]17 A2
SDA			13[		]16 A1
NC			14[		]15 A0


basic wiring for output....
pin 12,13 to rpi i2c port
pin 10,15,16,17 to ground (15,16,17 = set address to 0x20)
pin 09,18 to v+ (5 volt)
all other GP pins are wired as required to control your output hardware (steppers?)
*/




#include "rtapi.h"
#ifdef RTAPI
#include "rtapi_app.h"
#endif
#include "rtapi_string.h"
#include "rtapi_errno.h"
#include "hal.h"
#ifndef HAL_RPI_H
#define HAL_RPI_H

/*
most of this copyright mungkie 2012
some stuff just copied from the BCM2835 pdf
 * driver for mcp23017 i2c io expander interfaced to raspberry pi
check broadcom specs page 29 onwards
http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf
 */
#include <unistd.h>
#include <fcntl.h>
#include <sys/mman.h>

#define IOBASE   0x20000000
//// omg, what a terrible hack for rev 2 boards!!!!
//#define BSC0_BASE (IOBASE + 0x205000)
#define BSC1_BASE (IOBASE + 0x804000)
#define BSC0_BASE (IOBASE + 0x804000)
#define GPIO_BASE (IOBASE + 0x200000)

/// page 28 of BCM2835 manual (strangeness as offsets are 32 bit not 8 bit??)
#define BSC0_C        *(bsc0.addr + 0x00)
#define BSC0_S        *(bsc0.addr + 0x01)
#define BSC0_DLEN    *(bsc0.addr + 0x02)
#define BSC0_A        *(bsc0.addr + 0x03)
#define BSC0_FIFO    *(bsc0.addr + 0x04)
#define BSC0_CDIV    *(bsc0.addr + 0x05)

#define BSC_C_I2CEN    (1 << 15)
#define BSC_C_INTR    (1 << 10)
#define BSC_C_INTT    (1 << 9)
#define BSC_C_INTD    (1 << 8)
#define BSC_C_ST    (1 << 7)
#define BSC_C_CLEAR    (1 << 4)
#define BSC_C_READ    1

#define START_READ    BSC_C_I2CEN|BSC_C_ST|BSC_C_CLEAR|BSC_C_READ
#define START_WRITE    BSC_C_I2CEN|BSC_C_ST

#define BSC_S_CLKT    (1 << 9)
#define BSC_S_ERR    (1 << 8)
#define BSC_S_RXF    (1 << 7)
#define BSC_S_TXE    (1 << 6)
#define BSC_S_RXD    (1 << 5)
#define BSC_S_TXD    (1 << 4)
#define BSC_S_RXR    (1 << 3)
#define BSC_S_TXW    (1 << 2)
#define BSC_S_DONE    (1 << 1)
#define BSC_S_TA    1

#define CLEAR_STATUS    BSC_S_CLKT|BSC_S_ERR|BSC_S_DONE

#define PAGESIZE 4096
#define BLOCK_SIZE 4096

struct bcm2835_peripheral {
	unsigned long addr_p;
	int mem_fd;
	void *map;
	volatile unsigned int *addr;
};

struct bcm2835_peripheral gpio = {GPIO_BASE};
//struct bcm2835_peripheral bsc0 = {BSC0_BASE};
struct bcm2835_peripheral bsc0 = {BSC1_BASE};
// Exposes the physical address defined in the passed structure using mmap on /dev/mem
int map_peripheral(struct bcm2835_peripheral *p)
{
   if ((p->mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
      rtapi_print_msg(RTAPI_MSG_ERR,"\n\n\nFailed to open /dev/mem, try checking permissions.\n");
      return -1;
   }

   p->map = mmap(
      NULL,
      BLOCK_SIZE,
      PROT_READ|PROT_WRITE,
      MAP_SHARED,
      p->mem_fd,  // File descriptor to physical memory virtual file '/dev/mem'
      p->addr_p      // Address in physical map that we want this memory block to expose
   );

   if (p->map == MAP_FAILED) {
        perror("mmap");
        return -1;
   }

   p->addr = (volatile unsigned int *)p->map;

   return 0;
}

void unmap_peripheral(struct bcm2835_peripheral *p) {
	munmap(p->map, BLOCK_SIZE);
	close(p->mem_fd);
}

void dump_bsc_status() {
unsigned int s = BSC0_S;

rtapi_print_msg(RTAPI_MSG_ERR,"BSC0_S: ERR=%d  RXF=%d  TXE=%d  RXD=%d  TXD=%d  RXR=%d  TXW=%d  DONE=%d  TA=%d\n",
	(s & BSC_S_ERR) != 0,
	(s & BSC_S_RXF) != 0,
	(s & BSC_S_TXE) != 0,
	(s & BSC_S_RXD) != 0,
	(s & BSC_S_TXD) != 0,
	(s & BSC_S_RXR) != 0,
	(s & BSC_S_TXW) != 0,
	(s & BSC_S_DONE) != 0,
	(s & BSC_S_TA) != 0 );
}

/* all this stuff is very bad and needs some sort of fix, ? 
I think there should be some RT_PREEMPT wait functions that may help?

or we may need to create some sort of wake event so that data is written to the i2c device at a regular interval!
*/


#if 0
#define wait_i2c_done() 
#else
void wait_i2c_done() {
        //Wait till done, let's use a timeout just in case
#if 1
        int timeout = 5;
        while((!((BSC0_S) & BSC_S_DONE)) && --timeout) {
            usleep(30);
        }
        if(timeout == 0)
            rtapi_print_msg(RTAPI_MSG_ERR,"wait_i2c_done() timeout. Something went wrong.\n");
#endif
}
#endif

#define write_bank0(b) \
	BSC0_DLEN = 2; \
	BSC0_FIFO = 0x12; \
	BSC0_FIFO = b ; \
	BSC0_S = CLEAR_STATUS; \
	BSC0_C = START_WRITE;  \
	wait_i2c_done();

#define write_bank1(b) \
	BSC0_DLEN = 2; \
	BSC0_FIFO = 0x13; \
	BSC0_FIFO = b ; \
	BSC0_S = CLEAR_STATUS; \
	BSC0_C = START_WRITE;  \
	wait_i2c_done();

#endif

//#include "rpi_i2c.h"
static int comp_id;

MODULE_AUTHOR("Mungkie");
MODULE_DESCRIPTION("Raspberry Pi Parallel Port Driver for EMC HAL using MCP23017 i2c IO expander");
MODULE_LICENSE("WTF");
static char *cfg = "0x20";	/* config string, default 1 output port at 0x20 */
RTAPI_MP_STRING(cfg, "config string");

#ifdef MODULE_INFO

MODULE_INFO(linuxcnc, "driver:mcp23017:raspberry IO via MCP23017 i2c IO expander");
MODULE_INFO(linuxcnc, "\\fBTwo banks of 8bit IO per chip\\fR Two chips on the board\n");
MODULE_INFO(linuxcnc, "Try to keep minimal banks defined as each extra bank requires 2 bytes data and the max i2c clock is 2M bits per second ");
MODULE_INFO(linuxcnc, "license:WTF");
#endif // MODULE_INFO

#if 1
static unsigned short parse_port_addr(char *cp)
{
    unsigned short result;

    /* initial value */
    result = 0;
    /* test for leading '0x' */
    if (cp[0] == '0') {
	if ((cp[1] == 'X') || (cp[1] == 'x')) {
	    /* leading '0x', skip it */
	    cp += 2;
	}
    }
    /* ok, now parse digits */
    while (*cp != '\0') {
	/* if char is a hex digit, add it to result */
	if ((*cp >= '0') && (*cp <= '9')) {
	    result <<= 4;
	    result += *cp - '0';
	} else if ((*cp >= 'A') && (*cp <= 'F')) {
	    result <<= 4;
	    result += (*cp - 'A') + 10;
	} else if ((*cp >= 'a') && (*cp <= 'f')) {
	    result <<= 4;
	    result += (*cp - 'a') + 10;
	} else {
	    /* not a valid hex digit */
	    return -1;
	}
	/* next char */
	cp++;
    }

    return result;
}
#endif

struct __comp_state {
    struct __comp_state *_next;
    hal_bit_t *in0;
    hal_bit_t *out0[8];
    hal_bit_t *out1[8];
int c;
};



struct __comp_state *__comp_inst=0;
struct __comp_state *__comp_first_inst=0, *__comp_last_inst=0;

static void _(struct __comp_state *__comp_inst, long period);
static int __comp_get_data_size(void);

static int export(char *prefix, long extra_arg) {
    char buf[HAL_NAME_LEN + 1];
    int r = 0;
    int sz = sizeof(struct __comp_state) + __comp_get_data_size();
    struct __comp_state *inst = hal_malloc(sz);
int pin, portnum=0;
    memset(inst, 0, sz);
inst->c=0;

for(pin=0;pin<8;pin++){
   /* export read only HAL pin for output data */
//rtapi_print_msg(RTAPI_MSG_ERR,"reg pin %i\n",pin);
    r = hal_pin_bit_newf(HAL_IN, &(inst->out0[pin]), comp_id,
            "%s.out0.pin-%02d-out", prefix, pin);
//rtapi_print_msg(RTAPI_MSG_ERR, "%s.out0.pin-%02d-out", prefix, pin);
    if (r != 0) {
	return r;
    }
}

    rtapi_snprintf(buf, sizeof(buf), "%s", prefix);

    r = hal_export_funct(buf, (void(*)(void *inst, long))_, inst, 0, 0, comp_id);
    if(r != 0) return r;
    if(__comp_last_inst) __comp_last_inst->_next = inst;
    __comp_last_inst = inst;
    if(!__comp_first_inst) __comp_first_inst = inst;


    return 0;
}

#if 0
void setHighPri (void)
{
struct sched_param sched ;

memset (&sched, 0, sizeof(sched)) ;
sched.sched_priority = 10 ;
if (sched_setscheduler (0, SCHED_RR, &sched))
printf ("Warning: Unable to set high priority\n") ;
}
#endif

static int default_count=1, count=0;
char *names[16] = {0,};
RTAPI_MP_INT(count, "number of mcp23017");
RTAPI_MP_ARRAY_STRING(names, 16, "names of mcp23017");


int rtapi_app_main(void) {
int r = 0;
int i, c ;

#if 0
//pthread_t displayThread ;
//setHighPri () ;
if(ioperm(CRT_IC,1,1)){		// error
exit(-1);
}

ioperm(CRT_IM,1,1);
ioperm(ATT_IW,1,1);
#endif


	rtapi_print_msg(RTAPI_MSG_ERR,"The config string is %s.\n",cfg);

#if 1
 if(map_peripheral(&gpio) == -1) {
	rtapi_print_msg(RTAPI_MSG_ERR,"Failed to map the physical GPIO registers into the virtual memory space.\n");
	return -1;
 }
 if(map_peripheral(&bsc0) == -1) {
	rtapi_print_msg(RTAPI_MSG_ERR,"Failed to map the physical BSC0 (I2C) registers into the virtual memory space.\n");
	return -1;
 }

/* BSC0 is on GPIO 0 & 1 */
	//*gpio.addr &= ~0x3f; // Mask out bits 0-5 of FSEL0 (i.e. force to zero)
//	*gpio.addr |= 0x24;  // Set bits 0-5 of FSEL0 to binary '100100'

//this should work for rev 2 as mask for i2c bus 1 bsc1
	*gpio.addr &= ~0xfff; // Mask out bits 0-5 of FSEL0 (i.e. force to zero)
	*gpio.addr |= 0x924;  // Set bits 0-5 of FSEL0 to binary '100100100100'

/// I THINK MAYBE THE MCP23017 ALSO NEEDS REGISTER SETUP SO THE OUTPUTS ARE LATCHED???

//// setup i2c speed 
/// I think this is very important to get right so we run fast enough and possibly using the i2c clock to set step speed?

/* from bcm2835 datasheet .......
Clock Divider
SCL = core clock / CDIV
Where core_clk is nominally 150 MHz. If CDIV is
set to 0, the divisor is 32768. CDIV is always
rounded down to an even number. The default
value should result in a 100 kHz I2C clock
frequency.
*/

{
unsigned short di= (unsigned short) BSC0_CDIV;

#define BSC_DIV       *(bsc0.addr + 0x14)
#define I2C_CLOCK_HZ	2000000 /* 2MHz i2c clock*/
#define I2C_TIMEOUT_MS	150
//bus_hz = clk_get_rate(clk); // this should be 150MHz?
//di = bus_hz / I2C_CLOCK_HZ;
//BSC_DIV  = 1000;
di=8000; // 150MHz/75 = 2MHz?
BSC0_CDIV = di;
}


// I2C Device Address 0x20 (this is set by wiring address lines on mcp23017 to ground) see page 8 of mcp datasheet
// the address in binary will be  0 1 0 0 A2 A1 A0 (A2/A1/A0 pins on chip)
// as all adress lines are low, BSCO_A= 0100000 = 0x20
// if A1 was high, address = 0100010 = 0x22
// we should get the i2c address from the cfg string but I have not sorted parsing yet...
//
	BSC0_A = 0x20;


// I think we should probably set MCP23017 to byte mode not sequential......

///  this sets ports a for outputs
	BSC0_DLEN = 2; 
	BSC0_FIFO = 0x00; // bank a direction register address
	BSC0_FIFO = 0x00 ; // all 0 = output
	BSC0_S = CLEAR_STATUS; 
	BSC0_C = START_WRITE;  
	wait_i2c_done();

///  this sets ports b for outputs
	BSC0_DLEN = 2; 
	BSC0_FIFO = 0x10; // bank b direction register address
	BSC0_FIFO = 0x00 ; // all 0 = output
	BSC0_S = CLEAR_STATUS; 
	BSC0_C = START_WRITE;  
	wait_i2c_done();

write_bank0( 0x00); 
write_bank1( 0x00);

#endif

    comp_id = hal_init("mcp23017");
    if(comp_id < 0) return comp_id;
    if(count && names[0]) {
        rtapi_print_msg(RTAPI_MSG_ERR,"count= and names= are mutually exclusive\n");
        return -EINVAL;
    }
    if(!count && !names[0]) count = default_count;
    if(count) {
        for(i=0; i<count; i++) {
            char buf[HAL_NAME_LEN + 1];
            rtapi_snprintf(buf, sizeof(buf), "mcp23017.%d", i);
        r = export(buf, i);
            if(r != 0) break;
       }
    } else {
        for(i=0; names[i]; i++) {
	r = export(names[i], i); if(r != 0) break; }
}
    if(r) {
        hal_exit(comp_id);
    } else {
        hal_ready(comp_id);
    }
//rtapi_print_msg(RTAPI_MSG_ERR,"hal %i, %i, %i\n\n\n",r,comp_id,count);

    return r;
}

void rtapi_app_exit(void) {

write_bank0( 0x00); 
write_bank1( 0x00); 
 unmap_peripheral(&gpio);
    unmap_peripheral(&bsc0);
    hal_exit(comp_id);
}

#undef FUNCTION
#define FUNCTION(name) static void name(struct __comp_state *__comp_inst, long period)
#undef EXTRA_SETUP
#define EXTRA_SETUP() static int extra_setup(struct __comp_state *__comp_inst, char *prefix, long extra_arg)
#undef EXTRA_CLEANUP
#define EXTRA_CLEANUP() static void extra_cleanup(void)
#undef fperiod
#define fperiod (period * 1e-9)

#undef rpi_step_count
#define rpi_step_count (__comp_inst->c)

unsigned char 	outdata = 0x00;
unsigned char 	mask = 0x01;

FUNCTION(_) { /* out = in0 || in1; */ 

#if 0
// debuging info 
rpi_step_count++; 
if(rpi_step_count>1000)
{	rtapi_print_msg(RTAPI_MSG_ERR,"%i\n",rpi_step_count);	rpi_step_count=0;	}
#endif

{ 
int b;
outdata = 0x00;
mask = 0x01;
	/* assemble output byte for data port from 8 source variables */
// dont know whichway is fastest??
#if 0
	for (b = 0; b < 8; b++) {
	    if ((*(__comp_inst->out0[b]))) {
		outdata |= mask;
	    }
	    mask <<= 1;
	}
#else
outdata |= *(__comp_inst->out0[7]) ? mask : 0x00; 	mask <<=1;
outdata |= *(__comp_inst->out0[6]) ? mask : 0x00;	mask <<=1;
outdata |= *(__comp_inst->out0[5]) ? mask : 0x00;	mask <<=1;
outdata |= *(__comp_inst->out0[4]) ? mask : 0x00;	mask <<=1;
outdata |= *(__comp_inst->out0[3]) ? mask : 0x00;	mask <<=1;
outdata |= *(__comp_inst->out0[2]) ? mask : 0x00;	mask <<=1;
outdata |= *(__comp_inst->out0[1]) ? mask : 0x00;	mask <<=1;
outdata |= *(__comp_inst->out0[0]) ? mask : 0x00;	
#endif

#if 1
write_bank0( outdata); 
#endif
}
}

static int __comp_get_data_size(void) { return 0; }