Subversion Repositories shark

/*
 * Project: S.Ha.R.K.
 *
 * Coordinators:
 *   Giorgio Buttazzo    <giorgio@sssup.it>
 *   Paolo Gai           <pj@gandalf.sssup.it>
 *
 * Authors     :
 *   Giacomo Guidi       <giacomo@gandalf.sssup.it>
 *   (see the web pages for full authors list)
 *
 * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
 *
 * http://www.sssup.it
 * http://retis.sssup.it
 * http://shark.sssup.it
 */

/*
 * Copyright (C) 2002 Paolo Gai
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#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

#define barrier() __asm__ __volatile__("" ::: "memory");

#define SERVO_TIMEOUT 100000 /* us */

#define SERVO_PORT 1

int timer_expired = 0;
int timeout_event;
unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8};

void set_timer_expired(void *arg)
{
  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) {
      com_write(port,THR,b);
      return 0;
    } else {
      return -1;
    }
}

int com_receive(unsigned port)
{
    while ((com_read(port,LSR) & 1) == 0 && !timer_expired)
      barrier();
    if (!timer_expired) {
      return((int)(com_read(port,RBR)));
    } else {
      return -1;
    }
}

int servo_set_RS232_baudrate(int baud)
{

 
  return 0;

}

int servo_get_RS232_baudrate(void)
{

  return 0;

}

int servo_store_RS232_buadrate(void)
{

  return 0;

}

int servo_set_period(int period)
{

  return 0;

}

int servo_get_period(void)
{

  return 0;

}

int servo_store_period(void)
{

  return 0;

}

/* 1000.1100 */
int servo_get_setup_switch(void)
{
  struct timespec current_time;
  unsigned char b;
  int err,res;                                                                                                                        
  timer_expired = 0;
  kern_gettime(&current_time);
  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);  

  b = 0x8C;
  err = com_send(SERVO_PORT, b);
  err = com_receive(SERVO_PORT);
  if (err != (int)(b)) timer_expired = 1;
  res = com_receive(SERVO_PORT);

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return res;
  else
    return -1;

}

/* 1000.111s */
int servo_set_RC5_switch(int data)
{
  struct timespec current_time;
  unsigned char b;
  int err;
                                                                                                                             
  timer_expired = 0;
  kern_gettime(&current_time);
  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  b = 0x8E | (data & 0x01);
  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;

}

/* 0000.0ppp:0000.vvvv:vvvv.vvvv */
int servo_set_angle(int servo, int angle)
{

  struct timespec current_time;
  unsigned char b;
  int err;

  if (servo > 7) return -1;

  timer_expired = 0;
  kern_gettime(&current_time);
  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
 
  b = 0x00 | (servo & 0x07);
  err = com_send(SERVO_PORT, b);
  err = com_receive(SERVO_PORT);
  if (err != (int)(b)) timer_expired = 1;

  b = 0x00 | ((angle >> 8) & 0x0F);
  err = com_send(SERVO_PORT, b);
  err = com_receive(SERVO_PORT);
  if (err != (int)(b)) timer_expired = 1;

  b = 0x00 | (angle & 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;

}

/* 0000.1ppp */
int servo_get_angle(int servo)
{
  struct timespec current_time;
  unsigned char b;
  int err,res;
                                                                                                                             
  if (servo > 7) return -1;
                                                                                                                             
  timer_expired = 0;
  kern_gettime(&current_time);
  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  b = 0x08 | (servo & 0x07);
  err = com_send(SERVO_PORT, b);
  err = com_receive(SERVO_PORT);
  if (err != (int)(b)) timer_expired = 1;
  res = com_receive(SERVO_PORT) << 8;
  res |= com_receive(SERVO_PORT);

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return res;
  else
    return -1;

}

/* 0100:0aaa */
int servo_get_analog(int port)
{

  struct timespec current_time;
  unsigned char b;
  int err,res;
                                                                                                                             
  if (port > 4) return -1;
                                                                                                                             
  timer_expired = 0;
  kern_gettime(&current_time);
  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  b = 0x40 | (port & 7);
  err = com_send(SERVO_PORT, b);
  err = com_receive(SERVO_PORT);
  if (err != (int)(b)) timer_expired = 1;
  res = com_receive(SERVO_PORT) << 8;
  res |= com_receive(SERVO_PORT);

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return res;
  else
    return -1;

}