#include <rtapi_slab.h>
#include <rtapi_io.h>
#include "rtapi.h"
#include "rtapi_app.h"
#include "hal.h"
#include "hal_parport.h"
#define MAX_BUS 3
#define EPSILON 1e-20
MODULE_AUTHOR("John Kasunich");
MODULE_DESCRIPTION("HAL driver for Universal PWM Controller");
MODULE_LICENSE("GPL");
int port_addr[MAX_BUS] = { 0x378, [1 ... MAX_BUS-1] = -1 };
hal_parport_t port_registration[MAX_BUS];
RTAPI_MP_ARRAY_INT(port_addr, MAX_BUS, "port address(es) for EPP bus(es)");
int extradac[MAX_BUS*8] = {
-1,-1,-1,-1,-1,-1,-1,-1,
-1,-1,-1,-1,-1,-1,-1,-1,
-1,-1,-1,-1,-1,-1,-1,-1 };
RTAPI_MP_ARRAY_INT(extradac, MAX_BUS*8, "bus/slot locations of extra DAC modules");
int extradout[MAX_BUS*8] = {
-1,-1,-1,-1,-1,-1,-1,-1,
-1,-1,-1,-1,-1,-1,-1,-1,
-1,-1,-1,-1,-1,-1,-1,-1 };
RTAPI_MP_ARRAY_INT(extradout, MAX_BUS*8, "bus/slot locations of extra dig out modules");
int timestamp[MAX_BUS*8] = {
-1,-1,-1,-1,-1,-1,-1,-1,
-1,-1,-1,-1,-1,-1,-1,-1,
-1,-1,-1,-1,-1,-1,-1,-1 };
RTAPI_MP_ARRAY_INT(timestamp, MAX_BUS*8, "bus/slot locations of timestamped encoders");
int enc_clock[MAX_BUS*8] = {
-1,-1,-1,-1,-1,-1,-1,-1,
-1,-1,-1,-1,-1,-1,-1,-1,
-1,-1,-1,-1,-1,-1,-1,-1 };
RTAPI_MP_ARRAY_INT(enc_clock, MAX_BUS*8, "bus/slot locations of encoder clock settings");
int epp_dir[MAX_BUS] = {0 , [1 ... MAX_BUS-1] = 0 };
RTAPI_MP_ARRAY_INT(epp_dir, MAX_BUS, "EPP is commanded port direction");
#define SPPDATA(addr) addr
#define STATUSPORT(addr) addr+1
#define CONTROLPORT(addr) addr+2
#define ADDRPORT(addr) addr+3
#define DATAPORT(addr) addr+4
#define NUM_SLOTS 16
#define SLOT_SIZE 16
#define SLOT_ID_OFFSET 15
#define ENCCNT0 0x00
#define ENCCNT1 0x03
#define ENCCNT2 0x06
#define ENCCNT3 0x09
#define ENCCTRL 0x03
#define ENCRATE 0x04
#define ENCCLOCK 0x05
#define ENCISR 0x0C
#define ENCINDX 0x0D
#define ENCLOAD 0x00
#define ENCTS 0x10
#define ENCTS1 0x11
#define ENCTB 0x18
#define ENCTB1 0x19
#define DAC_0 0x00
#define DAC_1 0x02
#define DAC_2 0x04
#define DAC_3 0x06
#define DAC_MODE_0 0x4C
#define DAC_WRITE_0 0x48
#define DAC_MODE_1 0x5C
#define DAC_WRITE_1 0x58
#define PWM_GEN_0 0x10
#define PWM_GEN_1 0x12
#define PWM_GEN_2 0x14
#define PWM_GEN_3 0x16
#define PWM_CTRL_0 0x1C
#define PWM_FREQ_LO 0x1D
#define PWM_FREQ_HI 0x1E
#define RATE_GEN_0 0x10
#define RATE_GEN_1 0x13
#define RATE_GEN_2 0x16
#define RATE_GEN_3 0x19
#define RATE_CTRL_0 0x1C
#define RATE_SETUP_0 0x1D
#define RATE_WIDTH_0 0x1E
#define UxC_DINA 0x0D
#define UxC_DINB 0x0E
#define UxC_ESTOP_IN 0x0E
#define UxC_EXTRA 0x0F
#define EXTRA_UNUSED 0
#define EXTRA_DAC 1
#define EXTRA_DOUT 2
#define UxC_DOUTA 0x1F
#define UxC_ESTOP_OUT 0x1F
#define UxC_SLAVE 0x06
#define DIO_DINA 0x00
#define DIO_DINB 0x01
#define DIO_ESTOP_IN 0x02
#define DIO_DOUTA 0x00
#define DIO_ESTOP_OUT 0x02
typedef struct {
hal_bit_t *data;
hal_bit_t invert;
} dout_t;
typedef struct {
hal_bit_t *data;
hal_bit_t *data_not;
} din_t;
typedef struct {
hal_bit_t *enable;
hal_float_t *vel;
hal_float_t scale;
hal_float_t max_vel;
hal_float_t freq;
} stepgen_t;
typedef struct {
stepgen_t sg[4];
hal_u32_t setup_time_ns;
hal_u32_t pulse_width_ns;
hal_u32_t pulse_space_ns;
} stepgens_t;
#define BOOT_NORMAL 0
#define BOOT_REV 1
#define BOOT_FWD 2
typedef struct {
hal_bit_t *enable;
hal_float_t *value;
hal_float_t scale;
hal_float_t max_dc;
hal_float_t min_dc;
hal_float_t duty_cycle;
hal_bit_t bootstrap;
unsigned char boot_state;
unsigned char old_enable;
} pwmgen_t;
typedef struct {
pwmgen_t pg[4];
hal_float_t freq;
hal_float_t old_freq;
unsigned short period;
double period_recip;
} pwmgens_t;
typedef struct {
hal_float_t *value;
hal_float_t scale;
} DAC_t;
typedef struct {
DAC_t pg[4];
} DACs_t;
typedef union {
DAC_t dac;
dout_t douts[8];
} extra_t;
typedef struct {
hal_float_t *position;
hal_s32_t *count;
hal_s32_t *delta;
hal_s32_t prevdir;
hal_float_t scale;
hal_bit_t *index;
hal_bit_t *index_enable;
signed long oldreading;
unsigned int indres;
unsigned int indrescnt;
hal_float_t *vel;
hal_float_t min_speed;
hal_u32_t counts_since_timeout;
unsigned short old_timestamp;
unsigned short timestamp;
} encoder_t;
#define MAX_FUNCT 10
struct slot_data_s;
typedef void (slot_funct_t)(struct slot_data_s *slot);
typedef struct slot_data_s {
unsigned char id;
unsigned char ver;
unsigned char strobe;
unsigned char slot_base;
unsigned int port_addr;
rtapi_u32 read_bitmap;
unsigned char num_rd_functs;
unsigned char rd_buf[32];
slot_funct_t *rd_functs[MAX_FUNCT];
rtapi_u32 write_bitmap;
unsigned char num_wr_functs;
unsigned char wr_buf[32];
slot_funct_t *wr_functs[MAX_FUNCT];
dout_t *digout;
din_t *digin;
stepgens_t *stepgen;
pwmgens_t *pwmgen;
encoder_t *encoder;
DACs_t *DAC;
int extra_mode;
extra_t *extra;
unsigned int use_timestamp;
unsigned int enc_freq ;
} slot_data_t;
typedef struct {
int busnum;
unsigned char have_master;
unsigned int last_digout;
unsigned int last_digin;
unsigned int last_stepgen;
unsigned int last_pwmgen;
unsigned int last_encoder;
unsigned int last_DAC;
unsigned int last_extraDAC;
char slot_valid[NUM_SLOTS];
slot_data_t slot_data[NUM_SLOTS];
} bus_data_t;
static bus_data_t *bus_array[MAX_BUS];
static int comp_id;
static long read_period;
static int slotnum;
static int currentbus;
static void read_all(void *arg, long period);
static void write_all(void *arg, long period);
static void read_digins(slot_data_t *slot);
static void write_digouts(slot_data_t *slot);
static void write_stepgens(slot_data_t *slot);
static void write_pwmgens(slot_data_t *slot);
static void read_encoders(slot_data_t *slot);
static void write_DACs(slot_data_t *slot);
static void write_extraDAC(slot_data_t *slot);
#if 0#endif
static int ClrTimeout(unsigned int port_addr);
static unsigned short SelRead(unsigned char epp_addr, unsigned int port_addr);
static unsigned short ReadMore(unsigned int port_addr);
static void SelWrt(unsigned char byte, unsigned char epp_addr, unsigned int port_addr);
static void WrtMore(unsigned char byte, unsigned int port_addr);
static rtapi_u32 block(int min, int max);
static int add_rd_funct(slot_funct_t *funct, slot_data_t *slot, rtapi_u32 cache_bitmap );
static int add_wr_funct(slot_funct_t *funct, slot_data_t *slot, rtapi_u32 cache_bitmap );
static int export_UxC_digin(slot_data_t *slot, bus_data_t *bus);
static int export_UxC_digout(slot_data_t *slot, bus_data_t *bus);
static int export_PPMC_digin(slot_data_t *slot, bus_data_t *bus);
static int export_PPMC_digout(slot_data_t *slot, bus_data_t *bus);
static int export_USC_stepgen(slot_data_t *slot, bus_data_t *bus);
static int export_UPC_pwmgen(slot_data_t *slot, bus_data_t *bus);
static int export_PPMC_DAC(slot_data_t *slot, bus_data_t *bus);
static int export_encoders(slot_data_t *slot, bus_data_t *bus);
static int export_extra_dac(slot_data_t *slot, bus_data_t *bus);
static int export_extra_dout(slot_data_t *slot, bus_data_t *bus);
static int export_timestamp(slot_data_t *slot, bus_data_t *bus);
void rtapi_app_exit(void);
int rtapi_app_main(void)
{
int msg, rv, rv1, busnum, slotnum, n, boards;
int bus_slot_code, need_extra_dac, need_extra_dout, need_timestamp;
int idcode, id, ver;
bus_data_t *bus;
slot_data_t *slot;
char buf[HAL_NAME_LEN + 1];
comp_id = hal_init("hal_ppmc");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "PPMC: ERROR: hal_init() failed\n");
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: installing driver\n");
msg = rtapi_get_msg_level();
rtapi_set_msg_level(RTAPI_MSG_INFO);
n = 0;
rv = 0;
for ( busnum = 0 ; busnum < MAX_BUS ; busnum++ ) {
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: bus %d epp_dir = %d\n",busnum, epp_dir[busnum]);
bus_array[busnum] = NULL;
if ( port_addr[busnum] == -1 ) {
continue;
}
rv = hal_parport_get(comp_id, &port_registration[busnum],
port_addr[busnum], 0, PARPORT_MODE_EPP);
if(rv < 0)
return rv;
port_addr[busnum] = port_registration[busnum].base;
if(port_registration[busnum].base_hi)
rtapi_outb(0x80, port_registration[busnum].base_hi + 2);
n++;
}
if ( n == 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: no ports specified\n");
hal_exit(comp_id);
return -1;
}
for ( busnum = 0 ; busnum < MAX_BUS ; busnum++ ) {
if ( port_addr[busnum] == -1 ) {
continue;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"PPMC: checking EPP bus %d at port %04X\n",
busnum, port_addr[busnum]);
boards = 0;
bus = rtapi_kmalloc(sizeof(bus_data_t), RTAPI_GFP_KERNEL);
if (bus == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: kmalloc() failed\n");
rv = -1;
continue;
}
bus->busnum = busnum;
bus->have_master = 0;
bus->last_digout = 0;
bus->last_digin = 0;
bus->last_stepgen = 0;
bus->last_pwmgen = 0;
bus->last_DAC = 0;
bus->last_encoder = 0;
bus->last_extraDAC = 0;
for ( slotnum = 0 ; slotnum < NUM_SLOTS ; slotnum ++ ) {
bus->slot_valid[slotnum] = 0;
slot = &(bus->slot_data[slotnum]);
slot->id = 0;
slot->ver = 0;
slot->strobe = 0;
slot->slot_base = slotnum * SLOT_SIZE;
slot->port_addr = port_addr[busnum];
slot->read_bitmap = 0;
slot->write_bitmap = 0;
for ( n = 0 ; n < 32 ; n++ ) {
slot->rd_buf[n] = 0;
slot->wr_buf[n] = 0;
}
slot->num_rd_functs = 0;
slot->num_wr_functs = 0;
for ( n = 0 ; n < MAX_FUNCT ; n++ ) {
slot->rd_functs[n] = NULL;
slot->wr_functs[n] = NULL;
}
slot->digout = NULL;
slot->digin = NULL;
slot->stepgen = NULL;
slot->pwmgen = NULL;
slot->DAC = NULL;
slot->encoder = NULL;
slot->extra_mode = EXTRA_UNUSED;
slot->extra = NULL;
}
for ( slotnum = 0 ; slotnum < NUM_SLOTS ; slotnum ++ ) {
slot = &(bus->slot_data[slotnum]);
rv1 = 0;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: slot %d: ", slotnum);
idcode = SelRead(slot->slot_base+SLOT_ID_OFFSET, slot->port_addr);
if ((idcode == 0)||(idcode == 0xFF)||((idcode&0x0f) == 0x0f)) {
slot->id = 0;
slot->ver = 0;
rtapi_print_msg(RTAPI_MSG_INFO, "nothing detected at addr %x reads %x\n",
slotnum,idcode);
ClrTimeout(slot->port_addr);
continue;
}
rtapi_print_msg(RTAPI_MSG_INFO, "ID code: %02X ", idcode);
slot->id = id = idcode & 0xF0;
slot->ver = ver = idcode & 0x0F;
slot->use_timestamp = 0;
slot->enc_freq = 0;
bus->slot_valid[slotnum] = 1;
switch ( id ) {
case 0x10:
boards++;
bus_slot_code = (busnum << 4) | slotnum;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC encoder card %x\n",bus_slot_code);
need_timestamp = 0;
for ( n = 0; n < MAX_BUS*8 ; n++ ) {
if ( timestamp[n] == bus_slot_code ) {
need_timestamp = 1;
timestamp[n] = -1;
}
}
if ( need_timestamp ) {
rv1 += export_timestamp(slot, bus);
}
for ( n = 0; n < MAX_BUS*8 ; n++ ) {
if ( (enc_clock[n] & 0xff) == bus_slot_code) {
if (slot->ver < 4) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC encoder does not support adjustable encoder clock, ignoring\n");
}
slot->enc_freq = (enc_clock[n]) >> 8; }
}
rv1 += export_encoders(slot, bus);
if (slot->ver >= 4) slotnum++;
break;
case 0x20:
boards++;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC DAC card\n");
rv1 += export_PPMC_DAC(slot, bus);
break;
case 0x30:
boards++;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC Digital I/O card\n");
rv1 += export_PPMC_digin(slot, bus);
rv1 += export_PPMC_digout(slot, bus);
break;
case 0x40:
boards++;
rtapi_print_msg(RTAPI_MSG_INFO, "Univ. Stepper Controller\n");
rv1 += export_UxC_digin(slot, bus);
rv1 += export_UxC_digout(slot, bus);
rv1 += export_USC_stepgen(slot, bus);
rv1 += export_encoders(slot, bus);
bus_slot_code = (busnum << 4) | slotnum;
need_extra_dac = 0;
need_extra_dout = 0;
for ( n = 0; n < MAX_BUS*8 ; n++ ) {
if ( extradac[n] == bus_slot_code ) {
need_extra_dac = 1;
extradac[n] = -1;
}
if ( extradout[n] == bus_slot_code ) {
need_extra_dout = 1;
extradout[n] = -1;
}
}
if ( need_extra_dac && need_extra_dout ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: Can't have extra DAC and DOUT on same slot\n");
} else if ( need_extra_dac ) {
rv1 += export_extra_dac(slot, bus);
} else if ( need_extra_dout ) {
rv1 += export_extra_dout(slot, bus);
}
slotnum++;
break;
case 0x50:
boards++;
rtapi_print_msg(RTAPI_MSG_INFO, "Univ. PWM Controller\n");
rv1 += export_UxC_digin(slot, bus);
rv1 += export_UxC_digout(slot, bus);
rv1 += export_UPC_pwmgen(slot, bus);
bus_slot_code = (busnum << 4) | slotnum;
need_extra_dac = 0;
need_extra_dout = 0;
need_timestamp = 0;
for ( n = 0; n < MAX_BUS*8 ; n++ ) {
if ( extradac[n] == bus_slot_code ) {
need_extra_dac = 1;
extradac[n] = -1;
}
if ( extradout[n] == bus_slot_code ) {
need_extra_dout = 1;
extradout[n] = -1;
}
if ( timestamp[n] == bus_slot_code ) {
need_timestamp = 1;
timestamp[n] = -1;
}
}
if ( need_extra_dac && need_extra_dout ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: Can't have extra DAC and DOUT on same slot\n");
} else if ( need_extra_dac ) {
rv1 += export_extra_dac(slot, bus);
} else if ( need_extra_dout ) {
rv1 += export_extra_dout(slot, bus);
}
if ( need_timestamp ) {
rv1 += export_timestamp(slot, bus);
}
rv1 += export_encoders(slot, bus);
slotnum++;
break;
default:
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: Check Parallel Port connection.\n");
bus->slot_valid[slotnum] = 0;
rv1 = -1;
break;
}
rtapi_print_msg(RTAPI_MSG_INFO,"read cache bitmap: %08x\n", slot->read_bitmap );
rtapi_print_msg(RTAPI_MSG_INFO,"write cache bitmap: %08x\n", slot->write_bitmap );
}
if ( rv1 != 0 ) {
rv = -1;
continue;
}
if ( boards == 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: no boards found on bus %d, port %04X\n",
busnum, port_addr[busnum] );
rv = -1;
continue;
}
rtapi_snprintf(buf, sizeof(buf), "ppmc.%d.read", busnum);
rv1 = hal_export_funct(buf, read_all, &(bus_array[busnum]),
1, 0, comp_id);
if (rv1 != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: read funct export failed\n");
rv = -1;
continue;
}
rtapi_snprintf(buf, sizeof(buf), "ppmc.%d.write", busnum);
rv1 = hal_export_funct(buf, write_all, &(bus_array[busnum]),
1, 0, comp_id);
if (rv1 != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: write funct export failed\n");
rv = -1;
continue;
}
bus_array[busnum] = bus;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: bus %d complete\n", busnum);
}
for ( n = 0 ; n < MAX_BUS*8 ; n++ ) {
if ( extradac[n] != -1 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: no USC/UPC for extra dac at bus %d, slot %d\n",
extradac[n]>>4, extradac[n] & 0x0F );
rv = -1;
}
if ( extradout[n] != -1 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: no USC/UPC for extra douts at bus %d, slot %d\n",
extradout[n]>>4, extradout[n] & 0x0F );
rv = -1;
}
}
rtapi_set_msg_level(msg);
if ( rv != 0 ) {
rtapi_app_exit();
return rv;
}
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: driver installed\n");
hal_ready(comp_id);
return 0;
}
void rtapi_app_exit(void)
{
int busnum, n, m;
bus_data_t *bus;
rtapi_print_msg(RTAPI_MSG_ERR, "PPMC: shutting down\n");
for ( busnum = 0 ; busnum < MAX_BUS ; busnum++ ) {
if ( bus_array[busnum] != NULL ) {
bus = bus_array[busnum];
bus_array[busnum] = NULL;
for ( n = 0 ; n < 256 ; n += 16 ) {
SelWrt(0, n, bus->slot_data[0].port_addr);
for ( m = 1 ; m < 32 ; m++ ) {
WrtMore(0, bus->slot_data[0].port_addr);
}
}
rtapi_kfree(bus);
}
}
for(busnum = 0; busnum < MAX_BUS; busnum++) {
hal_parport_release(&port_registration[busnum]);
}
hal_exit(comp_id);
}
static void read_all(void *arg, long period)
{
bus_data_t *bus;
slot_data_t *slot;
int functnum, addr_ok;
unsigned char n, eppaddr;
rtapi_u32 bitmap;
read_period = period;
bus = *(bus_data_t **)(arg);
if ( bus == NULL ) {
return;
}
for ( slotnum = 0 ; slotnum < NUM_SLOTS ; slotnum++ ) {
currentbus = bus->busnum;
if ( bus->slot_valid[slotnum] ) {
slot = &(bus->slot_data[slotnum]);
if ( slot->strobe == 1 ) {
SelWrt(0x20, slot->slot_base + ENCRATE, slot->port_addr);
SelWrt(0x20, slot->slot_base + ENCRATE, slot->port_addr);
SelWrt(0x00, slot->slot_base + ENCRATE, slot->port_addr);
}
addr_ok = 0;
bitmap = slot->read_bitmap;
n = 0;
while ( bitmap ) {
if ( bitmap & 1 ) {
if ( addr_ok ) {
slot->rd_buf[n] = ReadMore(slot->port_addr);
} else {
eppaddr = slot->slot_base + n;
slot->rd_buf[n] = SelRead(eppaddr, slot->port_addr);
addr_ok = 1;
}
} else {
addr_ok = 0;
}
n++;
bitmap >>= 1;
}
for ( functnum = 0 ; functnum < slot->num_rd_functs ; functnum++ ) {
(slot->rd_functs[functnum])(slot);
}
}
}
}
static void write_all(void *arg, long period)
{
bus_data_t *bus;
slot_data_t *slot;
int slotnum, functnum, addr_ok;
unsigned char n, eppaddr;
rtapi_u32 bitmap;
bus = *(bus_data_t **)(arg);
if ( bus == NULL ) {
return;
}
for ( slotnum = 0 ; slotnum < NUM_SLOTS ; slotnum++ ) {
if ( bus->slot_valid[slotnum] ) {
currentbus = bus->busnum;
slot = &(bus->slot_data[slotnum]);
for ( functnum = 0 ; functnum < slot->num_wr_functs ; functnum++ ) {
(slot->wr_functs[functnum])(slot);
}
addr_ok = 0;
bitmap = slot->write_bitmap;
n = 0;
while ( bitmap ) {
if ( bitmap & 1 ) {
if ( addr_ok ) {
WrtMore(slot->wr_buf[n], slot->port_addr);
} else {
eppaddr = slot->slot_base + n;
SelWrt(slot->wr_buf[n], eppaddr, slot->port_addr);
addr_ok = 1;
}
} else {
addr_ok = 0;
}
n++;
bitmap >>= 1;
}
}
}
}
static void read_digins(slot_data_t *slot)
{
int b;
unsigned char indata, mask;
indata = slot->rd_buf[UxC_DINA];
b = 0;
mask = 0x01;
while ( b < 8 ) {
*(slot->digin[b].data) = indata & mask;
*(slot->digin[b].data_not) = !(indata & mask);
mask <<= 1;
b++;
}
indata = slot->rd_buf[UxC_DINB];
mask = 0x01;
while ( b < 16 ) {
*(slot->digin[b].data) = indata & mask;
*(slot->digin[b].data_not) = !(indata & mask);
mask <<= 1;
b++;
}
}
static void write_digouts(slot_data_t *slot)
{
int b;
unsigned char outdata, mask;
outdata = 0x00;
mask = 0x01;
for (b = 0; b < 8; b++) {
if ((*(slot->digout[b].data)) && (!slot->digout[b].invert)) {
outdata |= mask;
}
if ((!*(slot->digout[b].data)) && (slot->digout[b].invert)) {
outdata |= mask;
}
mask <<= 1;
}
slot->wr_buf[UxC_DOUTA] = outdata;
}
static void read_PPMC_digins(slot_data_t *slot)
{
int b;
unsigned char indata, mask;
indata = slot->rd_buf[DIO_DINA];
b = 0;
mask = 0x01;
while ( b < 8 ) {
*(slot->digin[b].data) = indata & mask;
*(slot->digin[b].data_not) = !(indata & mask);
mask <<= 1;
b++;
}
indata = slot->rd_buf[DIO_DINB];
mask = 0x01;
while ( b < 16 ) {
*(slot->digin[b].data) = indata & mask;
*(slot->digin[b].data_not) = !(indata & mask);
mask <<= 1;
b++;
}
if (slot->digin[b].data != NULL) {
indata = slot->rd_buf[DIO_ESTOP_IN];
mask = 0x01;
while ( b < 18 ) {
*(slot->digin[b].data) = indata & mask;
*(slot->digin[b].data_not) = !(indata & mask);
mask <<= 1;
b++;
}
}
}
static void write_PPMC_digouts(slot_data_t *slot)
{
int b;
unsigned char outdata, mask;
outdata = 0x00;
mask = 0x01;
for (b = 0; b < 8; b++) {
if ((*(slot->digout[b].data)) && (!slot->digout[b].invert)) {
outdata |= mask;
}
if ((!*(slot->digout[b].data)) && (slot->digout[b].invert)) {
outdata |= mask;
}
mask <<= 1;
}
slot->wr_buf[DIO_DOUTA] = outdata;
if (slot->digout[8].data != NULL) { outdata = 0; if ((*(slot->digout[8].data)) && (!slot->digout[8].invert)) {
outdata =1;
}
if ((!*(slot->digout[8].data)) && (slot->digout[8].invert)) {
outdata |= 1;
}
slot->wr_buf[DIO_ESTOP_OUT] = outdata;
}
else slot->wr_buf[DIO_ESTOP_OUT] = 2; }
static void read_encoders(slot_data_t *slot)
{
int i, byteindex, byteindx2;
double vel; union pos_tag {
signed long l;
struct byte_tag {
signed char b0;
signed char b1;
signed char b2;
signed char b3;
} byte;
} pos, oldpos;
union time_tag {
unsigned short s;
struct byte2_tag {
unsigned char b0;
unsigned char b1;
} byte;
} timebase, timestamp;
unsigned short delta_time;
if (slot->use_timestamp) {
byteindex = ENCTB;
timebase.byte.b0 = (unsigned char)slot->rd_buf[byteindex++];
timebase.byte.b1 = (unsigned char)slot->rd_buf[byteindex];
}
byteindex = ENCCNT0;
byteindx2 = ENCTS;
for (i = 0; i < 4; i++) {
slot->encoder[i].indrescnt++;
oldpos.l = slot->encoder[i].oldreading;
pos.byte.b0 = (signed char)slot->rd_buf[byteindex++];
pos.byte.b1 = (signed char)slot->rd_buf[byteindex++];
pos.byte.b2 = (signed char)slot->rd_buf[byteindex++];
pos.byte.b3 = oldpos.byte.b3;
if ((oldpos.byte.b2 & 0xc0) == 0xc0 && (pos.byte.b2 == 0))
pos.byte.b3++;
else
if ((oldpos.byte.b2 == 0) && (pos.byte.b2 & 0xc0) == 0xc0)
pos.byte.b3--;
*(slot->encoder[i].delta) = pos.l - slot->encoder[i].oldreading;
vel = (pos.l - slot->encoder[i].oldreading) /
(read_period * 1e-9 * slot->encoder[i].scale);
if ( (slot->rd_buf[ENCISR] & ( 1 << i )) != 0 ) {
*(slot->encoder[i].index) = 1;
if (slot->ver >= 2) {
if ( ((slot->encoder[0].indres & ( 1 << i )) != 0) &&
(slot->encoder[i].indrescnt > 3)) {
*(slot->encoder[i].index_enable) = 0;
if ( pos.byte.b2 < 0 ) {
pos.byte.b3 = 0xFF;
} else {
pos.byte.b3 = 0;
}
oldpos.byte.b3 = pos.byte.b3;
}
}
} else {
*(slot->encoder[i].index) = 0;
}
slot->encoder[i].oldreading = pos.l;
*(slot->encoder[i].count) = pos.l;
if (slot->encoder[i].scale < 0.0) {
if (slot->encoder[i].scale > -EPSILON)
slot->encoder[i].scale = -1.0;
} else {
if (slot->encoder[i].scale < EPSILON)
slot->encoder[i].scale = 1.0;
}
*(slot->encoder[i].position) = pos.l / slot->encoder[i].scale;
if (slot->use_timestamp) {
slot->encoder[i].old_timestamp = slot->encoder[i].timestamp;
timestamp.byte.b0 = slot->rd_buf[byteindx2++];
timestamp.byte.b1 = slot->rd_buf[byteindx2++];
slot->encoder[i].timestamp = timestamp.s;
if (*(slot->encoder[i].delta) != 0.0) {
delta_time = timestamp.s - slot->encoder[i].old_timestamp;
delta_time = delta_time & 0xffff;
if (slot->encoder[i].counts_since_timeout < 2) {
slot->encoder[i].counts_since_timeout++;
*(slot->encoder[i].vel) = vel; } else {
vel = *(slot->encoder[i].delta) / (delta_time * 1e-6 * slot->encoder[i].scale);
*(slot->encoder[i].vel) = vel;
}
if (((slot->encoder[i].prevdir > 0) && (*(slot->encoder[i].delta) < 0)) ||
((slot->encoder[i].prevdir < 0) && (*(slot->encoder[i].delta) > 0))) {
*(slot->encoder[i].vel) = 0.0;
}
} else {
if (slot->encoder[i].counts_since_timeout) {
delta_time = timebase.s - timestamp.s;
delta_time = delta_time & 0xffff;
if (delta_time < 65500) {
vel = 1.0 / (slot->encoder[i].scale * delta_time * 1e-6);
if (vel < 0.0) vel = -vel;
if (vel < *(slot->encoder[i].vel)) {
*(slot->encoder[i].vel) = vel;
}
if (-vel > *(slot->encoder[i].vel)) {
*(slot->encoder[i].vel) = -vel;
}
} else {
slot->encoder[i].counts_since_timeout = 0;
*(slot->encoder[i].vel) = 0;
}
} else {
*(slot->encoder[i].vel) = 0;
}
}
} else {
*(slot->encoder[i].vel) = vel; }
if (*(slot->encoder[i].delta) > 0) slot->encoder[i].prevdir = 1; if (*(slot->encoder[i].delta) < 0) slot->encoder[i].prevdir = -1;
}
}
static void write_encoders(slot_data_t *slot)
{
int i;
if ( slot->ver < 2 ) {
return;
}
for (i = 0; i < 4; i++) {
if ( *(slot->encoder[i].index_enable) ) {
if ((slot->encoder[0].indres & (1 << i)) == 0) {
slot->encoder[i].indrescnt = 0;
(slot->encoder[0].indres) |= (1 << i);
}
} else {
(slot->encoder[0].indres) &= ~(1 << i);
}
}
slot->wr_buf[ENCINDX] = slot->encoder[0].indres;
}
static unsigned int ns2cp( hal_u32_t *pns, unsigned int min_ns )
{
int ns, cp;
ns = *pns;
if ( ns < min_ns ) ns = min_ns;
if ( ns > 25400 ) ns = 25400;
cp = ns / 100;
ns = cp * 100;
*pns = ns;
return cp;
}
static void write_stepgens(slot_data_t *slot)
{
int n, reverse, run, pulse_width, pulse_space, setup_time;
unsigned int divisor;
stepgen_t *sg;
double bd_max_freq, ch_max_freq, abs_scale, freq;
unsigned char control_byte;
pulse_width = ns2cp(&(slot->stepgen->pulse_width_ns), 200);
slot->wr_buf[RATE_WIDTH_0] = 256 - pulse_width;
pulse_space = ns2cp(&(slot->stepgen->pulse_space_ns), 300);
setup_time = ns2cp(&(slot->stepgen->setup_time_ns), 200);
slot->wr_buf[RATE_SETUP_0] = 256 - setup_time;
bd_max_freq = 10000000.0 / (pulse_width + pulse_space);
control_byte = 0;
for ( n = 0 ; n < 4 ; n++ ) {
sg = &(slot->stepgen->sg[n]);
if ( sg->scale < 0.0 ) {
if ( sg->scale > -EPSILON ) {
sg->scale = -1.0;
}
abs_scale = -sg->scale;
} else {
if ( sg->scale < EPSILON ) {
sg->scale = 1.0;
}
abs_scale = sg->scale;
}
ch_max_freq = bd_max_freq;
if (sg->max_vel <= 0.0) {
sg->max_vel = 0.0;
} else {
if ( (sg->max_vel * abs_scale) > ch_max_freq) {
sg->max_vel = ch_max_freq / abs_scale;
} else {
ch_max_freq = sg->max_vel * abs_scale;
}
}
freq = *(sg->vel) * sg->scale;
if ( *(sg->enable) != 0 ) {
run = 1;
} else {
run = 0;
}
reverse = 0;
if ( freq < 0.0 ) {
freq = -freq;
reverse = 1;
}
if ( freq > ch_max_freq ) {
freq = ch_max_freq;
divisor = 10000000.0 / freq;
} else if ( freq < (10000000.0/16777215.0) ) {
freq = 0.0;
divisor = 16777215;
run = 0;
} else {
divisor = ( 10000000.0 / freq ) + 0.5;
freq = 10000000.0 / divisor;
}
control_byte >>= 2;
if ( run ) {
control_byte |= 0x80;
}
if ( reverse ) {
sg->freq = -freq;
} else {
sg->freq = freq;
control_byte |= 0x40;
}
divisor -= 4;
slot->wr_buf[RATE_GEN_0+(n*3)] = divisor & 0xff;
divisor >>= 8;
slot->wr_buf[RATE_GEN_0+(n*3)+1] = divisor & 0xff;
divisor >>= 8;
slot->wr_buf[RATE_GEN_0+(n*3)+2] = divisor & 0xff;
}
slot->wr_buf[RATE_CTRL_0] = control_byte;
}
static void write_pwmgens(slot_data_t *slot)
{
int n, reverse;
unsigned int period, start, len, stop;
pwmgen_t *pg;
double freq, dc, abs_dc;
unsigned char control_byte;
if ( slot->pwmgen->freq == 0.0 ) {
slot->pwmgen->old_freq = slot->pwmgen->freq;
slot->wr_buf[PWM_CTRL_0] = 0;
return;
}
if ( slot->pwmgen->freq != slot->pwmgen->old_freq ) {
freq = slot->pwmgen->freq;
if ( freq < 153.0 ) {
freq = 153.0;
}
if ( freq > 500000.0 ) {
freq = 500000.0;
}
if (slot->ver >= 3) period = (40000000.0 / freq) + 0.5; else
period = (10000000.0 / freq) + 0.5;
freq = 10000000.0 / period;
slot->pwmgen->freq = freq;
slot->pwmgen->old_freq = freq;
slot->pwmgen->period = period;
slot->pwmgen->period_recip = 1.0 / period;
}
start = 65536 - slot->pwmgen->period;
slot->wr_buf[PWM_FREQ_LO] = start & 0xFF;
slot->wr_buf[PWM_FREQ_HI] = (start >> 8) & 0xFF;
control_byte = 0;
for ( n = 0 ; n < 4 ; n++ ) {
pg = &(slot->pwmgen->pg[n]);
if ( pg->scale < 0.0 ) {
if ( pg->scale > -EPSILON ) {
pg->scale = -1.0;
}
} else {
if ( pg->scale < EPSILON ) {
pg->scale = 1.0;
}
}
dc = *(pg->value) / pg->scale;
if ( pg->bootstrap != 0 ) {
if (( *(pg->enable) != 0 ) && ( pg->old_enable == 0 )) {
pg->boot_state = BOOT_FWD;
}
pg->old_enable = *(pg->enable);
switch(pg->boot_state) {
case BOOT_NORMAL:
break;
case BOOT_REV:
dc = -0.05;
pg->boot_state = BOOT_NORMAL;
break;
case BOOT_FWD:
dc = 0.05;
pg->boot_state = BOOT_REV;
break;
default:
pg->boot_state = BOOT_NORMAL;
break;
}
}
reverse = 0;
if ( dc < 0.0 ) {
reverse = 1;
abs_dc = -dc;
} else {
abs_dc = dc;
}
if (( pg->min_dc > 1.0 ) || ( pg->min_dc < 0.0 )) {
pg->min_dc = 0.0;
}
if (( pg->max_dc > 1.0 ) || ( pg->max_dc < 0.0 )) {
pg->max_dc = 1.0;
}
if ( pg->min_dc >= pg->max_dc ) {
pg->min_dc = 0.0;
pg->max_dc = 1.0;
}
if ( abs_dc > pg->max_dc ) {
abs_dc = pg->max_dc;
} else if ( abs_dc < pg->min_dc ) {
abs_dc = pg->min_dc;
}
len = ( abs_dc * slot->pwmgen->period ) + 0.5;
abs_dc = len * slot->pwmgen->period_recip;
control_byte >>= 2;
if ( *(pg->enable) != 0 ) {
control_byte |= 0x80;
}
if ( reverse ) {
pg->duty_cycle = -abs_dc;
} else {
pg->duty_cycle = abs_dc;
control_byte |= 0x40;
}
stop = 65535 - len;
slot->wr_buf[PWM_GEN_0+(n*2)] = stop & 0xff;
stop >>= 8;
slot->wr_buf[PWM_GEN_0+(n*2)+1] = stop & 0xff;
}
slot->wr_buf[PWM_CTRL_0] = control_byte;
}
static void write_DACs(slot_data_t *slot)
{
int n;
DAC_t *pg;
long dc;
double volts;
for ( n = 0 ; n < 4 ; n++ ) {
pg = &(slot->DAC->pg[n]);
if ( pg->scale < 0.0 ) {
if ( pg->scale > -EPSILON ) {
pg->scale = -1.0;
}
} else {
if ( pg->scale < EPSILON ) {
pg->scale = 1.0;
}
}
volts = *(pg->value) / pg->scale;
dc = (long) (((volts / 10.0) * 0x7FFF)+0x8000);
if (dc > 0xffff)
{
dc = 0xffff;
}
if (dc < 0 )
{
dc = 0;
}
slot->wr_buf[DAC_0+(n*2)] = dc & 0xff; dc >>= 8;
slot->wr_buf[DAC_0+(n*2)+1] = dc & 0xff; }
}
static void write_extraDAC(slot_data_t *slot)
{
DAC_t *pg;
long dc;
double volts;
pg = &(slot->extra->dac);
if ( pg->scale < 0.0 ) {
if ( pg->scale > -EPSILON ) {
pg->scale = -1.0;
}
} else {
if ( pg->scale < EPSILON ) {
pg->scale = 1.0;
}
}
volts = *(pg->value) / pg->scale;
if (volts < 0.0 ) volts = -volts;
dc = (long) ((volts / 10.0) * 0xff);
if (dc > 0xff) {
dc = 0xff;
}
if (dc < 0 ) {
dc = 0;
}
slot->wr_buf[UxC_EXTRA] = dc & 0xff; }
static void write_extra_dout(slot_data_t *slot)
{
dout_t *pg;
int b;
unsigned char outdata, mask;
outdata = 0x00;
mask = 0x01;
for (b = 0; b < 8; b++) {
pg = &(slot->extra->douts[b]);
if ((*(pg->data)) && (!pg->invert)) {
outdata |= mask;
}
if ((!*(pg->data)) && (pg->invert)) {
outdata |= mask;
}
mask <<= 1;
}
slot->wr_buf[UxC_EXTRA] = outdata; }
static rtapi_u32 block(int min, int max)
{
int n;
rtapi_u32 mask;
mask = 0;
for ( n = min ; n <= max ; n++ ) {
mask |= ( 1 << n );
}
return mask;
}
static int add_rd_funct(slot_funct_t *funct, slot_data_t *slot,
rtapi_u32 cache_bitmap )
{
if ( slot->num_rd_functs >= MAX_FUNCT ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: too many read functions\n");
return -1;
}
slot->rd_functs[slot->num_rd_functs++] = funct;
slot->read_bitmap |= cache_bitmap;
return 0;
}
static int add_wr_funct(slot_funct_t *funct, slot_data_t *slot,
rtapi_u32 cache_bitmap )
{
if ( slot->num_wr_functs >= MAX_FUNCT ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: too many write functions\n");
return -1;
}
slot->wr_functs[slot->num_wr_functs++] = funct;
slot->write_bitmap |= cache_bitmap;
return 0;
}
static int export_UxC_digin(slot_data_t *slot, bus_data_t *bus)
{
int retval, n;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: exporting UxC digital inputs\n");
slot->digin = hal_malloc(16 * sizeof(din_t));
if (slot->digin == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: hal_malloc() failed\n");
return -1;
}
for ( n = 0 ; n < 16 ; n++ ) {
retval = hal_pin_bit_newf(HAL_OUT, &(slot->digin[n].data), comp_id,
"ppmc.%d.din.%02d.in", bus->busnum, bus->last_digin);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(slot->digin[n].data_not), comp_id,
"ppmc.%d.din.%02d.in-not", bus->busnum, bus->last_digin);
if (retval != 0) {
return retval;
}
bus->last_digin++;
}
add_rd_funct(read_digins, slot, block(UxC_DINA, UxC_DINB));
return 0;
}
static int export_UxC_digout(slot_data_t *slot, bus_data_t *bus)
{
int retval, n;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: exporting UxC digital outputs\n");
SelWrt(0, slot->slot_base+UxC_DOUTA, slot->port_addr);
if (bus->last_digout > 7) { rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: slave UxC addr %x\n",slot->slot_base+UxC_SLAVE);
SelWrt(1,slot->slot_base+UxC_SLAVE,slot->port_addr);
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: slave UxC # %d\n",bus->last_digout);
}
slot->digout = hal_malloc(8 * sizeof(dout_t));
if (slot->digout == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: hal_malloc() failed\n");
return -1;
}
for ( n = 0 ; n < 8 ; n++ ) {
retval = hal_pin_bit_newf(HAL_IN, &(slot->digout[n].data), comp_id,
"ppmc.%d.dout.%02d.out", bus->busnum, bus->last_digout);
if (retval != 0) {
return retval;
}
retval = hal_param_bit_newf(HAL_RW, &(slot->digout[n].invert), comp_id,
"ppmc.%d.dout.%02d-invert", bus->busnum, bus->last_digout);
if (retval != 0) {
return retval;
}
slot->digout[n].invert = 0;
bus->last_digout++;
}
add_wr_funct(write_digouts, slot, block(UxC_DOUTA, UxC_DOUTA));
return 0;
}
static int export_PPMC_digin(slot_data_t *slot, bus_data_t *bus)
{
int retval, n;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: exporting PPMC digital inputs\n");
slot->digin = hal_malloc(18 * sizeof(din_t)); if (slot->digin == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: hal_malloc() failed\n");
return -1;
}
for ( n = 0 ; n < 16 ; n++ ) {
retval = hal_pin_bit_newf(HAL_OUT, &(slot->digin[n].data), comp_id,
"ppmc.%d.din.%02d.in", bus->busnum, bus->last_digin);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(slot->digin[n].data_not), comp_id,
"ppmc.%d.din.%02d.in-not", bus->busnum, bus->last_digin);
if (retval != 0) {
return retval;
}
bus->last_digin++;
}
if (bus->last_digin < 31) {
retval = hal_pin_bit_newf(HAL_OUT, &(slot->digin[16].data), comp_id,
"ppmc.%d.din.estop.in", bus->busnum);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(slot->digin[16].data_not), comp_id,
"ppmc.%d.din.estop.in-not", bus->busnum);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(slot->digin[17].data), comp_id,
"ppmc.%d.din.fault.in", bus->busnum);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(slot->digin[17].data_not), comp_id,
"ppmc.%d.din.fault.in-not", bus->busnum);
if (retval != 0) {
return retval;
}
add_rd_funct(read_PPMC_digins, slot, block(DIO_DINA, DIO_ESTOP_IN));
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: exporting as MASTER D In\n");
}
else {
add_rd_funct(read_PPMC_digins, slot, block(DIO_DINA, DIO_DINB));
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: exporting as SLAVE D In\n");
}
return 0;
}
static int export_PPMC_digout(slot_data_t *slot, bus_data_t *bus)
{
int retval, n;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: exporting PPMC digital outputs\n");
SelWrt(0, slot->slot_base+DIO_DOUTA, slot->port_addr);
if (bus->last_digout > 7) { rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: slave DIO addr %x\n",slot->slot_base+DIO_ESTOP_OUT);
SelWrt(2,slot->slot_base+DIO_ESTOP_OUT,slot->port_addr);
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: slave DIO # %d\n",bus->last_digout);
}
slot->digout = hal_malloc(9 * sizeof(dout_t)); if (slot->digout == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: hal_malloc() failed\n");
return -1;
}
for ( n = 0 ; n < 8 ; n++ ) {
retval = hal_pin_bit_newf(HAL_IN, &(slot->digout[n].data), comp_id,
"ppmc.%d.dout.%02d.out", bus->busnum, bus->last_digout);
if (retval != 0) {
return retval;
}
retval = hal_param_bit_newf(HAL_RW, &(slot->digout[n].invert), comp_id,
"ppmc.%d.dout.%02d.invert", bus->busnum, bus->last_digout);
if (retval != 0) {
return retval;
}
slot->digout[n].invert = 0;
bus->last_digout++;
}
if (bus->last_digout < 15) { rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: master DIO at # %d\n",bus->last_digout);
retval = hal_pin_bit_newf(HAL_IN, &(slot->digout[8].data), comp_id,
"ppmc.%d.dout.Estop.out", bus->busnum);
if (retval != 0) {
return retval;
}
retval = hal_param_bit_newf(HAL_RW, &(slot->digout[8].invert), comp_id,
"ppmc.%d.dout.Estop.invert", bus->busnum);
if (retval != 0) {
return retval;
}
slot->digout[8].invert = 0;
add_wr_funct(write_PPMC_digouts, slot, block(DIO_DOUTA, DIO_ESTOP_OUT));
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: exporting as MASTER D Out\n");
}
else {
add_wr_funct(write_PPMC_digouts, slot, block(DIO_DOUTA, DIO_DOUTA));
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: exporting as SLAVE D Out\n");
SelWrt(2,slot->slot_base+DIO_ESTOP_OUT,slot->port_addr);
}
return 0;
}
static int export_USC_stepgen(slot_data_t *slot, bus_data_t *bus)
{
int retval, n;
stepgen_t *sg;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: exporting step generators\n");
slot->stepgen = hal_malloc(sizeof(stepgens_t));
if (slot->stepgen == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: hal_malloc() failed\n");
return -1;
}
retval = hal_param_u32_newf(HAL_RW, &(slot->stepgen->setup_time_ns), comp_id,
"ppmc.%d.stepgen.%02d-%02d.setup-time-ns", bus->busnum, bus->last_stepgen, bus->last_stepgen+3);
if (retval != 0) {
return retval;
}
slot->stepgen->setup_time_ns = 10000;
retval = hal_param_u32_newf(HAL_RW, &(slot->stepgen->pulse_width_ns), comp_id,
"ppmc.%d.stepgen.%02d-%02d.pulse-width-ns", bus->busnum, bus->last_stepgen, bus->last_stepgen+3);
if (retval != 0) {
return retval;
}
slot->stepgen->pulse_width_ns = 4000;
retval = hal_param_u32_newf(HAL_RW, &(slot->stepgen->pulse_space_ns), comp_id,
"ppmc.%d.stepgen.%02d-%02d.pulse-space-min-ns", bus->busnum, bus->last_stepgen, bus->last_stepgen+3);
if (retval != 0) {
return retval;
}
slot->stepgen->pulse_space_ns = 4000;
for ( n = 0 ; n < 4 ; n++ ) {
sg = &(slot->stepgen->sg[n]);
retval = hal_pin_bit_newf(HAL_IN, &(sg->enable), comp_id,
"ppmc.%d.stepgen.%02d.enable", bus->busnum, bus->last_stepgen);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IN, &(sg->vel), comp_id,
"ppmc.%d.stepgen.%02d.velocity", bus->busnum, bus->last_stepgen);
if (retval != 0) {
return retval;
}
retval = hal_param_float_newf(HAL_RW, &(sg->scale), comp_id,
"ppmc.%d.stepgen.%02d.scale", bus->busnum, bus->last_stepgen);
if (retval != 0) {
return retval;
}
sg->scale = 1.0;
retval = hal_param_float_newf(HAL_RW, &(sg->max_vel), comp_id,
"ppmc.%d.stepgen.%02d.max-vel", bus->busnum, bus->last_stepgen);
if (retval != 0) {
return retval;
}
sg->max_vel = 0.0;
retval = hal_param_float_newf(HAL_RO, &(sg->freq), comp_id,
"ppmc.%d.stepgen.%02d.freq", bus->busnum, bus->last_stepgen);
if (retval != 0) {
return retval;
}
bus->last_stepgen++;
}
add_wr_funct(write_stepgens, slot, block(RATE_GEN_0, RATE_WIDTH_0));
return 0;
}
static int export_UPC_pwmgen(slot_data_t *slot, bus_data_t *bus)
{
int retval, n;
pwmgen_t *pg;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: exporting PWM generators\n");
slot->pwmgen = hal_malloc(sizeof(pwmgens_t));
if (slot->pwmgen == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: hal_malloc() failed\n");
return -1;
}
retval = hal_param_float_newf(HAL_RW, &(slot->pwmgen->freq), comp_id,
"ppmc.%d.pwm.%02d-%02d.freq", bus->busnum, bus->last_pwmgen, bus->last_pwmgen+3);
if (retval != 0) {
return retval;
}
slot->pwmgen->freq = 0.0;
for ( n = 0 ; n < 4 ; n++ ) {
pg = &(slot->pwmgen->pg[n]);
retval = hal_pin_bit_newf(HAL_IN, &(pg->enable), comp_id,
"ppmc.%d.pwm.%02d.enable", bus->busnum, bus->last_pwmgen);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IN, &(pg->value), comp_id,
"ppmc.%d.pwm.%02d.value", bus->busnum, bus->last_pwmgen);
if (retval != 0) {
return retval;
}
retval = hal_param_float_newf(HAL_RW, &(pg->scale), comp_id,
"ppmc.%d.pwm.%02d.scale", bus->busnum, bus->last_pwmgen);
if (retval != 0) {
return retval;
}
pg->scale = 1.0;
retval = hal_param_float_newf(HAL_RW, &(pg->max_dc), comp_id,
"ppmc.%d.pwm.%02d.max-dc", bus->busnum, bus->last_pwmgen);
if (retval != 0) {
return retval;
}
pg->max_dc = 1.0;
retval = hal_param_float_newf(HAL_RW, &(pg->min_dc), comp_id,
"ppmc.%d.pwm.%02d.min-dc", bus->busnum, bus->last_pwmgen);
if (retval != 0) {
return retval;
}
pg->min_dc = 0.0;
retval = hal_param_float_newf(HAL_RO, &(pg->duty_cycle), comp_id,
"ppmc.%d.pwm.%02d.duty-cycle", bus->busnum, bus->last_pwmgen);
if (retval != 0) {
return retval;
}
retval = hal_param_bit_newf(HAL_RW, &(pg->bootstrap), comp_id,
"ppmc.%d.pwm.%02d.bootstrap", bus->busnum, bus->last_pwmgen);
if (retval != 0) {
return retval;
}
pg->bootstrap = 0;
pg->boot_state = 0;
pg->old_enable = 0;
bus->last_pwmgen++;
}
add_wr_funct(write_pwmgens, slot, block(PWM_GEN_0, PWM_GEN_3+1) | block(PWM_CTRL_0,PWM_FREQ_HI));
return 0;
}
static int export_PPMC_DAC(slot_data_t *slot, bus_data_t *bus)
{
int retval, n;
DAC_t *pg;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: exporting PPMC DAC\n");
slot->DAC = hal_malloc(sizeof(DACs_t));
if (slot->DAC == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: hal_malloc() failed\n");
return -1;
}
for ( n = 0 ; n < 4 ; n++ ) {
pg = &(slot->DAC->pg[n]);
retval = hal_pin_float_newf(HAL_IN, &(pg->value), comp_id,
"ppmc.%d.DAC.%02d.value", bus->busnum, bus->last_DAC);
if (retval != 0) {
return retval;
}
retval = hal_param_float_newf(HAL_RW, &(pg->scale), comp_id,
"ppmc.%d.DAC.%02d.scale", bus->busnum, bus->last_DAC);
if (retval != 0) {
return retval;
}
pg->scale = 1.0;
bus->last_DAC++;
}
add_wr_funct(write_DACs, slot, block(DAC_0, DAC_3+1));
return 0;
}
static int export_encoders(slot_data_t *slot, bus_data_t *bus)
{
int retval, n, m;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: exporting encoder pins / params\n");
SelWrt(0x00, slot->slot_base+ENCCTRL, slot->port_addr);
if ( bus->have_master == 0 ) {
slot->strobe = 1;
SelWrt(0x10, slot->slot_base+ENCRATE, slot->port_addr);
bus->have_master = 1;
} else {
slot->strobe = 0;
SelWrt(0x00, slot->slot_base+ENCRATE, slot->port_addr);
}
SelWrt(0xF0, slot->slot_base+ENCCTRL, slot->port_addr);
SelWrt(0x00, slot->slot_base+ENCLOAD, slot->port_addr);
WrtMore(0x00, slot->port_addr);
WrtMore(0x00, slot->port_addr);
ClrTimeout(slot->port_addr);
ClrTimeout(slot->port_addr);
ClrTimeout(slot->port_addr);
SelWrt(0x00, slot->slot_base+ENCCTRL, slot->port_addr);
SelWrt(0x00,slot->slot_base+ENCINDX, slot->port_addr);
slot->encoder = hal_malloc(4 * sizeof(encoder_t));
if (slot->encoder == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: hal_malloc() failed\n");
return -1;
}
slot->encoder[0].indres = 0;
if (slot->enc_freq != 0) {
switch (slot->enc_freq) {
case 1:
m = 0;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: setting encoder clock to 1 MHz.\n");
break;
case 2:
m = 1;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: setting encoder clock to 2.5 MHz.\n");
break;
case 5:
m = 2;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: setting encoder clock to 5 MHz.\n");
break;
case 10:
m = 3;
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: setting encoder clock to 10 MHz.\n");
break;
default:
m = 0;
rtapi_print_msg(RTAPI_MSG_ERR, "PPMC: invalid encoder clock setting.\n");
break;
}
SelWrt(m, slot->slot_base+ENCCLOCK, slot->port_addr); }
for ( n = 0 ; n < 4 ; n++ ) {
retval = hal_param_float_newf(HAL_RW, &(slot->encoder[n].scale), comp_id,
"ppmc.%d.encoder.%02d.scale", bus->busnum, bus->last_encoder);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_OUT, &(slot->encoder[n].position), comp_id,
"ppmc.%d.encoder.%02d.position", bus->busnum, bus->last_encoder);
if (retval != 0) {
return retval;
}
retval = hal_pin_s32_newf(HAL_OUT, &(slot->encoder[n].count), comp_id,
"ppmc.%d.encoder.%02d.count", bus->busnum, bus->last_encoder);
if (retval != 0) {
return retval;
}
retval = hal_pin_s32_newf(HAL_OUT, &(slot->encoder[n].delta), comp_id,
"ppmc.%d.encoder.%02d.delta", bus->busnum, bus->last_encoder);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(slot->encoder[n].index), comp_id,
"ppmc.%d.encoder.%02d.index", bus->busnum, bus->last_encoder);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_OUT, &(slot->encoder[n].vel), comp_id,
"ppmc.%d.encoder.%02d.velocity",bus->busnum,bus->last_encoder);
if (retval != 0) {
return retval;
}
if (slot->ver >= 2) {
retval = hal_pin_bit_newf(HAL_IO, &(slot->encoder[n].index_enable), comp_id,
"ppmc.%d.encoder.%02d.index-enable", bus->busnum, bus->last_encoder);
if (retval != 0) {
return retval;
}
if (slot->use_timestamp) {
retval = hal_param_float_newf(HAL_RW, &(slot->encoder[n].min_speed), comp_id,
"ppmc.%d.encoder.%02d.min-speed-estimate", bus->busnum, bus->last_encoder);
if (retval != 0) {
return retval;
}
}
}
bus->last_encoder++;
}
if (slot->use_timestamp) {
add_rd_funct(read_encoders, slot, block(ENCCNT0, ENCISR) | block(ENCTS, ENCTB+1));
} else {
add_rd_funct(read_encoders, slot, block(ENCCNT0, ENCISR));
}
add_wr_funct(write_encoders, slot, block(ENCINDX, ENCINDX));
return 0;
}
static int export_extra_dac(slot_data_t *slot, bus_data_t *bus)
{
int retval, n;
DAC_t *pg;
n=0;
if (slot->id == 0x40) n=1;
if (slot->id == 0x50 && slot->ver >= 2) n=1;
if ( n == 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: board firmware doesn't support 'extra' port\n");
return -1;
}
slot->extra = hal_malloc(sizeof(extra_t));
if (slot->extra == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: hal_malloc() failed\n");
return -1;
}
slot->extra_mode = EXTRA_DAC;
pg = &(slot->extra->dac);
retval = hal_pin_float_newf(HAL_IN, &(pg->value), comp_id,
"ppmc.%d.DAC8.%02d.value", bus->busnum, bus->last_extraDAC);
if (retval != 0) {
return retval;
}
retval = hal_param_float_newf(HAL_RW, &(pg->scale), comp_id,
"ppmc.%d.DAC8.%02d.scale", bus->busnum, bus->last_extraDAC);
if (retval != 0) {
return retval;
}
pg->scale = 1.0;
bus->last_extraDAC++;
add_wr_funct(write_extraDAC, slot, block(UxC_EXTRA, UxC_EXTRA));
return 0;
}
int export_timestamp(slot_data_t *slot, bus_data_t *bus)
{
int n;
n=0;
if ((slot->id == 0x10 || slot->id == 0x50) && slot->ver >= 4) n=1;
if ( n == 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: board firmware doesn't support encoder timestamp.\n");
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO, "PPMC: exporting encoder timestamp pins\n");
slot->use_timestamp = 1;
return 0;
}
static int export_extra_dout(slot_data_t *slot, bus_data_t *bus)
{
int retval, n;
dout_t *pg;
n=0;
if (slot->id == 0x40) n=1;
if (slot->id == 0x50 && slot->ver >= 2) n=1;
if ( n == 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: board firmware doesn't support 'extra' port\n");
return -1;
}
slot->extra = hal_malloc(sizeof(extra_t));
if (slot->extra == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"PPMC: ERROR: hal_malloc() failed\n");
return -1;
}
slot->extra_mode = EXTRA_DOUT;
for ( n = 0 ; n < 8 ; n++ ) {
pg = &(slot->extra->douts[n]);
retval = hal_pin_bit_newf(HAL_IN, &(pg->data), comp_id,
"ppmc.%d.dout.%02d.out", bus->busnum, bus->last_digout);
if (retval != 0) {
return retval;
}
retval = hal_param_bit_newf(HAL_RW, &(pg->invert), comp_id,
"ppmc.%d.dout.%02d.invert", bus->busnum, bus->last_digout);
if (retval != 0) {
return retval;
}
pg->invert = 0;
bus->last_digout++;
}
add_wr_funct(write_extra_dout, slot, block(UxC_EXTRA, UxC_EXTRA));
return 0;
}
#if 0#endif
static int ClrTimeout(unsigned int port_addr)
{
unsigned char r;
r = rtapi_inb(STATUSPORT(port_addr));
if (!(r & 0x01)) {
return 0;
}
r = rtapi_inb(STATUSPORT(port_addr));
rtapi_outb(r | 0x01, STATUSPORT(port_addr));
r = rtapi_inb(STATUSPORT(port_addr));
return !(r & 0x01);
}
static unsigned short SelRead(unsigned char epp_addr, unsigned int port_addr)
{
unsigned char b;
ClrTimeout(port_addr);
rtapi_outb(0x04,CONTROLPORT(port_addr));
rtapi_outb(epp_addr,ADDRPORT(port_addr));
if (epp_dir[currentbus] == 1) {
rtapi_outb(0x24,CONTROLPORT(port_addr));
}
b = rtapi_inb(DATAPORT(port_addr));
return b;
}
static unsigned short ReadMore(unsigned int port_addr)
{
unsigned char b;
b = rtapi_inb(DATAPORT(port_addr));
return b;
}
static void SelWrt(unsigned char byte, unsigned char epp_addr, unsigned int port_addr)
{
ClrTimeout(port_addr);
rtapi_outb(0x04,CONTROLPORT(port_addr));
rtapi_outb(epp_addr,ADDRPORT(port_addr));
rtapi_outb(byte,DATAPORT(port_addr));
return;
}
static void WrtMore(unsigned char byte, unsigned int port_addr)
{
rtapi_outb(byte,DATAPORT(port_addr));
return;
}