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;

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

void set_timer_expired(void *arg)

{

  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;

}

int servo_get_setup_switch(void)

{

  return 0;

}

int servo_set_RC5_switch(int data)

{

  return 0;

}

/* 0000.0ppp:0000.vvvv:vvvv.vvvv */

int servo_set_angle(int servo, int angle)

{

  struct timespec current_time;

  unsigned char b;

  int err,timeout_event;

  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)) return -1;

  b = 0x00 | ((angle >> 8) & 0x0F);

  err = com_send(SERVO_PORT, b);

  err = com_receive(SERVO_PORT);

  if (err != (int)(b)) return -1;

  b = 0x00 | (angle & 0xFF);

  err = com_send(SERVO_PORT, b);

  err = com_receive(SERVO_PORT);

  if (err != (int)(b)) return -1;  

  kern_event_delete(timeout_event);

  return 0;

}

/* 0000.1ppp */

int servo_get_angle(int servo)

{

  struct timespec current_time;

  unsigned char b;

  int err,res,timeout_event;

  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)) return -1;

  res = com_receive(SERVO_PORT);

  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,timeout_event;

  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)) return -1;

  res = com_receive(SERVO_PORT) << 8;

  res |= com_receive(SERVO_PORT);

  kern_event_delete(timeout_event);

  if (!timer_expired)

    return res;

  else

    return -1;

}