Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 716 → Rev 717

/shark/trunk/ports/servo/servo.c
38,26 → 38,12
 
#include <ll/sys/ll/ll-instr.h>
#include "kernel/kern.h"
#include "servo.h"
 
#define THR 0
#define RBR 0
#define IER 1
#define FCR 2
#define IIR 2
#define LCR 3
#define MCR 4
#define LSR 5
#define MSR 6
#define SPad 7
#include "drivers/scom.h"
#include "drivers/scomirq.h"
 
/* Parity value */
#define NONE 0
#define ODD 1
#define EVEN 3
#include "servo.h"
 
#define barrier() __asm__ __volatile__("" ::: "memory");
 
//#define SERVO_DEBUG
//#define SERVO_TIMEOUT_EVENT
 
149,7 → 135,11
int timer_expired = 0;
int timeout_event;
 
const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8};
#define RXTX_BUFF_MAX 100
static BYTE RXTX_buff[4][RXTX_BUFF_MAX];
static BYTE *RXTX_addr[4][RXTX_BUFF_MAX];
unsigned int RX_position[4] = {0,0,0,0};
unsigned int TX_position[4] = {0,0,0,0};
 
const int BaudTable[] = {
1200,
168,92 → 158,82
timeout_event = NIL;
timer_expired = 1;
}
unsigned com_read(unsigned port,unsigned reg)
{
unsigned b;
if (port > 3 || reg > 7) return(0);
b = ll_in(com_base[port]+reg);
return(b);
}
void com_write(unsigned port,unsigned reg,unsigned value)
{
if (port > 3 || reg > 7) return;
ll_out(com_base[port]+reg,value);
}
int com_send(unsigned port,BYTE b)
{
while ((com_read(port,LSR) & 32) == 0 && !timer_expired)
barrier();
if (!timer_expired) {
#ifdef SERVO_DEBUG
kern_printf("(SERVO WRITE p = %d b = %02x)",port,b);
#endif
com_write(port,THR,b);
return 0;
} else {
#ifdef SERVO_DEBUG
kern_printf("(WRITE TIMEOUT)");
#endif
return -1;
}
}
 
int com_receive(unsigned port)
{
int b;
void servo_rx_error(unsigned port) {
 
while ((com_read(port,LSR) & 1) == 0 && !timer_expired)
barrier();
if (!timer_expired) {
b = (int)(com_read(port,RBR));
#ifdef SERVO_DEBUG
kern_printf("(SERVO READ p = %d b = %02x)",port,b);
#endif
return b;
} else {
#ifdef SERVO_DEBUG
kern_printf("(READ TIMEOUT)");
#endif
return -1;
}
kern_printf("SERVO: RX ERROR COM%d\n",port);
 
}
 
int com_open(unsigned port,DWORD speed,BYTE parity,BYTE len,BYTE stop)
{
unsigned long div,b_mask;
/* Now set up the serial link */
b_mask = (parity & 3) * 8 + (stop & 1) * 4 + ((len - 5) & 3);
div = 115200L / speed;
/* Clear serial interrupt enable register */
com_write(port,IER,0);
/* Empty input buffer */
com_read(port,RBR);
/* Activate DLAB bit for speed setting */
com_write(port,LCR,0x80);
/* Load baud divisor */
com_write(port,0,div & 0x00FF);
div >>= 8;
com_write(port,1,div & 0x00FF);
/* Load control word (parity,stop bit,bit len) */
com_write(port,LCR,b_mask);
/* Attiva OUT1 & OUT2 */
com_write(port,MCR,0x0C);
void servo_indication(unsigned port, BYTE data) {
 
return 0;
if (RXTX_addr[port][RX_position[port]] == NULL) {
if (data != RXTX_buff[port][RX_position[port]])
servo_rx_error(port);
RX_position[port]++;
if (RX_position[port] >= RXTX_BUFF_MAX) RX_position[port] = 0;
if (TX_position[port] == RX_position[port])
servo_rx_error(port);
} else {
*RXTX_addr[port][RX_position[port]] = data;
RX_position[port]++;
if (RX_position[port] >= RXTX_BUFF_MAX) RX_position[port] = 0;
if (TX_position[port] == RX_position[port])
servo_rx_error(port);
}
 
cprintf("(TXRX:%d:%d)",TX_position[port],RX_position[port]);
 
}
 
int com_close(unsigned port)
{
void servo_confirm(unsigned port, BYTE msg_status) {
 
com_write(port,IER,0);
com_read(port,IIR);
com_read(port,RBR);
#ifdef SERVO_DEBUG
if (msg_status == COM_ERROR)
kern_printf("(SERVO:COM%d ERROR)",port);
#endif
 
}
 
int servo_send_msg(unsigned port, BYTE *msg, unsigned len_msg, BYTE *res, unsigned len_res) {
 
int i = 0, old;
old = TX_position[port];
 
cprintf("(SEND:%d:%d)",len_msg,len_res);
 
while(i < len_msg) {
RXTX_buff[port][TX_position[port]] = msg[i];
RXTX_addr[port][TX_position[port]] = NULL;
TX_position[port]++;
if (TX_position[port] >= RXTX_BUFF_MAX) TX_position[port] = 0;
if (TX_position[port] == RX_position[port]) {
TX_position[port] = old;
cprintf("Error\n");
return -1;
}
i++;
}
 
i = 0;
while(i < len_res) {
RXTX_buff[port][TX_position[port]] = 0;
RXTX_addr[port][TX_position[port]] = res+i;
TX_position[port]++;
if (TX_position[port] >= RXTX_BUFF_MAX) TX_position[port] = 0;
if (TX_position[port] == RX_position[port]) {
TX_position[port] = old;
cprintf("Error\n");
return -1;
}
i++;
}
 
com_irq_send(port, len_msg, msg);
 
cprintf("(TXRX:%d:%d)",TX_position[port],RX_position[port]);
 
return 0;
 
}
264,6 → 244,10
 
err = com_open((unsigned)(port), (DWORD)speed, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
 
com_init_irq((unsigned)(port));
 
com_set_functions(servo_confirm,servo_indication);
 
return err;
 
}
272,6 → 256,8
{
int err;
 
com_close_irq((unsigned)(port));
 
err = com_close((unsigned)(port));
 
return err;
909,11 → 895,8
/* 0000.Pppp:0000.vvvv:vvvv.vvvv */
int servo_set_angle_sec(int port, int servo, int angle_sec)
{
#ifdef SERVO_TIMEOUT_EVENT
struct timespec current_time;
#endif
unsigned char b;
int err, angle_tick;
unsigned char b[3];
int angle_tick;
int servo_port = (unsigned)(port);
 
if (servo > 15) return -1;
922,38 → 905,13
servo_table[port][servo].delta_tick /
(servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec)) * 1000 / TICK_LEN;
 
timer_expired = 0;
#ifdef SERVO_TIMEOUT_EVENT
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
#else
timeout_event = NIL;
#endif
b[0] = 0x00 | (servo & 0x0F);
b[1] = 0x00 | ((angle_tick >> 8) & 0x0F);
b[2] = 0x00 | (angle_tick & 0xFF);
servo_send_msg(servo_port, b, 3, NULL, 0);
 
b = 0x00 | (servo & 0x0F);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
return 0;
 
b = 0x00 | ((angle_tick >> 8) & 0x0F);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
b = 0x00 | (angle_tick & 0xFF);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return 0;
else
return -1;
 
}
 
/* 0010.Pppp */
/shark/trunk/ports/servo/makefile
14,7 → 14,7
 
OBJS = $(SERVO)
 
C_OPT += -I./include -I. -I..
C_OPT += -I./include -I. -I$(BASE)/drivers/serial/include
 
include $(BASE)/config/lib.mk