| 0,0 → 1,181 |
|---|
| /* |
| * 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 "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 TIMEOUT 100000 /* us */ |
| |
| int timer_expired = 0; |
| unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8}; |
| |
| void set_timer_expired(void) |
| { |
| |
| 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; |
| |
| } |
| |
| |
| int servo_set_angle(int servo, int angle) |
| { |
| |
| return 0; |
| |
| } |
| |
| int servo_get_angle(int servo) |
| { |
| |
| return 0; |
| |
| } |
| |
| int servo_get_analog(int port) |
| { |
| |
| return 0; |
| |
| } |
| |