BTnut System Software: bt-lego/bt-lego.c

00001 /*
00002  * Copyright (C) 2000-2004 by ETH Zurich
00003  *
00004  * Redistribution and use in source and binary forms, with or without
00005  * modification, are permitted provided that the following conditions
00006  * are met:
00007  *
00008  * 1. Redistributions of source code must retain the above copyright
00009  *    notice, this list of conditions and the following disclaimer.
00010  * 2. Redistributions in binary form must reproduce the above copyright
00011  *    notice, this list of conditions and the following disclaimer in the
00012  *    documentation and/or other materials provided with the distribution.
00013  * 3. Neither the name of the copyright holders nor the names of
00014  *    contributors may be used to endorse or promote products derived
00015  *    from this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY ETH ZURICH AND CONTRIBUTORS
00018  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00019  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00020  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL ETH ZURICH
00021  * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00022  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00023  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
00024  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
00025  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00026  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
00027  * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
00028  * SUCH DAMAGE.
00029  *
00030  * For additional information see http://www.btnode.ethz.ch
00031  *
00032  */
00049 /********************************************************/
00050 /* INCLUDES */
00051 /********************************************************/
00052 
00053 #include <stdio.h>           // FILE, fopen(), freopen, stdout, _fileno()
00054 #include <io.h>              // _ioctl()
00055 
00056 #include <sys/device.h>      // NutRegisterDevice()
00057 #include <sys/thread.h>
00058 #include <sys/timer.h>
00059 #include <sys/event.h>
00060 
00061 #include <hardware/btn-hardware.h> // btn_hardware_bt_on()
00062 #include <bt/bt_hci_defs.h>  // bt_addr_t
00063 #include <bt/bt_hci_api.h>   // bt_hci_init()
00064 #include <bt/bt_hci_cmds.h>  // bt_hci_read_bd_addr()
00065 #include <bt/bt_defs.h>      // struct btstack
00066 #include <bt/bt_psm.h>
00067 #include <bt/l2cap_cl.h>
00068 #include <dev/usartavr.h>    // BT_UART, APP_UART
00069 #include <suart/btn-suart.h> // S_UART
00070 #include <led/btn-led.h>
00071 
00072 #include <terminal/btn-terminal.h>
00073 #include <terminal/bt-cmds.h>
00074 #include <terminal/btn-cmds.h>
00075 #include <terminal/nut-cmds.h>
00076 
00077 
00078 
00079 /********************************************************/
00080 /* GLOBAL VARIABLES */
00081 /********************************************************/
00082 #define NPAIRS 3
00083 u_char masters[NPAIRS] = {0x31,0x29,0x6a};
00084 u_char slaves[NPAIRS]  = {0x8a,0x20,0x7e};
00085 
00086 bt_addr_t master_addr = {0x00,0x00,0x00,0x3f,0x04,0x00};
00087 bt_addr_t slave_addr  = {0x00,0x00,0x00,0x3f,0x04,0x00};
00088 
00089 struct btstack* bt_stack = NULL;
00090 bt_psm_t* psmux = NULL;
00091 
00092 FILE *uart, *uarts;
00093 
00094 int con_handle = -1;
00095 
00096 /********************************************************/
00097 /* INTERNAL FUNCTION DECLARATIONS */
00098 /********************************************************/
00099 void _init_btn(void);
00100 void _init_bt(void);
00101 int  _am_i_master(void);
00102 void _init_suart(void);
00103 void _init_duart(void);
00104 void _run_terminal(void);
00105 int  _open_con_to_slave(void);
00106 int  _parse_lego(u_char *, int);
00107 int _send_to_slave(u_char *, int);
00108 void _test_send(u_char*);
00109 
00110 
00111 
00112 /********************************************************/
00113 /* INTERNAL FUNCTION DEFINITIONS */
00114 /********************************************************/
00115 
00116 void _init_btn(void) {
00117     btn_hardware_init();
00118     btn_led_init(1);    
00119 }
00120 
00121 void _init_bt(void) {
00122     u_short acl_size;
00123     btn_hardware_bt_on();
00124     bt_stack = bt_hci_init(&BT_UART);
00125     if (bt_stack != NULL){
00126         acl_size = bt_acl_init(bt_stack, BT_HCI_PACKET_TYPE_DM3);
00127         psmux = bt_psm_init(bt_stack, 2, 4, acl_size);
00128         l2cap_cl_init(bt_stack, psmux, bt_hci_register_acl_cb);
00129     }
00130 }
00131 
00132 int _am_i_master(void) {
00133     int i;
00134     bt_addr_t my_addr;
00135     
00136     if (bt_stack == 0) 
00137         return 0; // stack not initialized
00138     bt_hci_get_local_bt_addr(bt_stack, my_addr);
00139     printf("my addr is %u\n",my_addr[0]);    
00140     for (i=0;i<NPAIRS;i++) {
00141         if (my_addr[0] == masters[i]) {
00142             master_addr[0] = masters[i];
00143             slave_addr[0]  = slaves[i];
00144             return 1;
00145         }
00146     }
00147     return 0;
00148 }
00149 
00150 void _init_suart(void) {
00151     u_long sbaud = 2400;
00152     u_long sparity = 1; // ODD
00153 
00154     NutRegisterDevice(&S_UART, 0, 0);
00155     uarts = fopen(S_UART.dev_name, "r+");
00156     _ioctl(_fileno(uarts), UART_SETSPEED, &sbaud);
00157     _ioctl(_fileno(uarts), UART_SETPARITY, &sparity);
00158 }
00159 
00160 void _init_duart(void) {
00161 //    u_long baud = 115200;
00162     u_long baud = 19200;
00163 
00164     NutRegisterDevice(&APP_UART, 0, 0);
00165     uart = freopen(APP_UART.dev_name, "r+",stdout);
00166     _ioctl(_fileno(uart), UART_SETSPEED, &baud);
00167 }
00168 
00169 
00170 void _run_terminal(void) {
00171 
00172     btn_terminal_init(stdout, "[bt-lego]$");
00173     btn_cmds_register_cmds();
00174     nut_cmds_register_cmds();
00175     if (bt_stack != 0) {
00176         bt_cmds_init(bt_stack);
00177         bt_cmds_register_cmds();
00178     }
00179     btn_terminal_register_cmd("legosend", _test_send);
00180     btn_terminal_run(BTN_TERMINAL_NOFORK, 0);   
00181 }
00182 
00183 int  _open_con_to_slave(void){
00184     long retval;
00185     if (psmux != NULL) {
00186         retval = bt_hci_create_connection(bt_stack, BT_HCI_SYNC, slave_addr, BT_HCI_PACKET_TYPE_DM1 | BT_HCI_PACKET_TYPE_DH1 | BT_HCI_PACKET_TYPE_DM3, 0, 0, BT_HCI_ROLE_SWITCH_CHANGE);
00187         return (int) retval;
00188     }
00189     return -1;
00190 }
00191 
00192 int  _parse_lego(u_char * buf, int len){
00193     static int state = 1;
00194     static u_char data = 0;
00195     int i;
00196 //  printf("parsing %d chars\n",len);
00197     for(i=0;i<len;i++) {
00198 //      printf("parsing %x in state %d\n",buf[i],state);
00199         switch (state) {
00200             case 1:
00201                 if (buf[i]==0x55 || buf[i]==0xd5) state = 2;
00202                 else state = 1;
00203                 data = 0;
00204                 break;
00205             case 2:
00206                 if (buf[i]==0xff) state = 3;
00207                 else 
00208                     if (buf[i]==0x00) state = 4;
00209                     else state = 1;
00210                 break;
00211             case 3:
00212                 if (buf[i]==0x00) state = 4;
00213                 else state = 1;
00214                 break;
00215             case 4:
00216                 if (buf[i]==0xf7) state = 5;
00217                 else state = 1;
00218                 break;
00219             case 5:
00220                 if (buf[i]==0x08) state = 6;
00221                 else state = 1;
00222                 break;
00223             case 6:
00224                 data = buf[i];
00225                 state = 7;
00226 //              printf("data is %u",data);
00227 //              printf(", ~data is %u",~data);
00228                 break;
00229             case 7:
00230 //              printf(", inv-data is %u",buf[i]);
00231 //              printf(", ~inv-data is %u",~buf[i]);
00232                 if (data+buf[i] == 255) {
00233                     state = 8;
00234 //                  printf("-> match!\n");
00235                 }
00236                 else {
00237                     state = 1;
00238 //                  printf("-> miss!\n");
00239                 }
00240                 break;
00241             case 8:
00242                 state = 9;
00243                 break;
00244             case 9:
00245                 state = 1;
00246                 return (int)data;
00247         }
00248     }
00249     return -state;
00250 }
00251 
00252 void _test_send(u_char* arg) {
00253     u_char data = 0xdd;
00254     printf_P(PSTR("test send.."));
00255     _send_to_slave(&data,1);
00256     printf_P(PSTR("ok\n"));
00257 }
00258 
00259 int _send_to_slave(u_char * outbuf, int len) {
00260     long retval;
00261     int i;
00262     bt_acl_com_pkt_t pkt;
00263     u_char *data = bt_acl_com_get_data_pointer(&pkt);
00264     //cast payload to acl_com
00265 //  printf("_send: con_handle=%d\n",con_handle);
00266     if (con_handle<0) {
00267         return -1;
00268     }
00269     for (i=0;i<len;i++) {
00270         data[i] = outbuf[i];
00271     }
00272 //      printf_P(PSTR("_send ..."));
00273     
00274     retval = bt_acl_com_send_packet(acl_stack, &pkt, (bt_hci_con_handle_t)con_handle, 3, 1);
00275     if (retval == BT_ERR_NO_CON) {
00276         printf_P(PSTR(" BT_ERR_NO_CON\n"));
00277         con_handle = -1;
00278         return -1;
00279     }
00280    if (retval == BT_ERR_BT_MODULE) {
00281         printf_P(PSTR(" BT_ERR_BT_MODULE\n"));
00282         return -1;
00283     }
00284 //      printf_P(PSTR(" ok\n"));
00285     return 0;
00286 }
00287 
00288 int _get_lego_msg(void)
00289 {
00290     int msg = -1;
00291     long res;
00292     u_short len;
00293     //acl-pkt
00294     bt_acl_com_pkt_t *p_pkt = 0;
00295     bt_hci_con_handle_t get_handle;
00296     u_char *data;
00297     u_char errors;
00298     errors = bt_acl_com_get_status(acl_stack);
00299     if (errors & BT_ERR_INVALID_SERV_NR) {
00300         DEBUGT("get: error: pkt with invalid service number arrived from module\n");
00301     } else if (errors & BT_ERR_INVALID_HANDLE) {
00302         DEBUGT("get: error: pkt with invalid con handle arrived from module\n");
00303     } else if (errors & BT_ERR_FRAGMENTED_ACL_PKT) {
00304         DEBUGT("get: error: fragmented acl pkt arrived from module\nplease change pkt type!\n");
00305     } else if (errors & BT_ERR_BUFFER_OVERFLOW) {
00306         DEBUGT("get: error: internal acl buffer overflowed\n");
00307     }
00308     res = bt_acl_com_get_packet(acl_stack, 3, &p_pkt, &get_handle, &len, 0);
00309     data = bt_acl_com_get_data_pointer(p_pkt);
00310 //    DEBUGT("pkt arrived from handle %d of len %d. Data:\n", get_handle, len);
00311 //    bt_acl_com_dump_data(data, len);
00312     msg = (int)data[0];
00313     bt_acl_com_free_packet(acl_stack, p_pkt);
00314     return msg;
00315 }
00316 
00317 void _send_to_lego(int msg) {
00318     u_char buffer[]={0x55,0xff,0x00,0xf7,0x08,0x00,0x00,0x00,0x00};
00319     buffer[5]=(u_char)msg;
00320     buffer[6]=~buffer[5];
00321     buffer[7]=(u_char)(0xf7+buffer[5]);
00322     buffer[8]=~buffer[7];
00323     _write(_fileno(uarts), buffer, 9);        
00324 }
00325 
00326 THREAD(lego_slave, arg)
00327 //void lego_slave(void)
00328 {
00329     int msg;
00330     
00331     for(;;){    
00332         msg = _get_lego_msg();
00333         if (msg>=0) {
00334 //          printf("received msg %d, forwarding...",msg);
00335             btn_led_add_pattern(BTN_LED_PATTERN_KNIGHT, 0, 1, 1);                           
00336             _send_to_lego(msg);
00337 //          printf_P(PSTR("ok\n"));
00338         }
00339             printf("received msg %d\n",msg);
00340 //      NutSleep(1000);
00341     }
00342 } 
00343 
00344 THREAD(lego_master, arg)
00345 //void lego_master(void)
00346 {
00347     int got, msg;
00348     u_char inbuf[20];
00349     u_char c_msg;
00350 
00351     for (;;) {
00352         printf_P(PSTR("Connection to slave ..."));
00353         while (con_handle<0){
00354             con_handle = (int)  _open_con_to_slave();
00355             if (con_handle<0)
00356                 printf(" err code -%x ",-con_handle);
00357         }   
00358         btn_led_set(3);
00359         printf_P(PSTR("ok\n")); 
00360         for(;;){
00361             got = _read(_fileno(uarts), inbuf, 20);
00362             if (con_handle<0) {
00363                 printf_P(PSTR("Connection dropped\n"));
00364                 btn_led_clear(3);
00365                 break; //get new connection
00366             }
00367             msg = _parse_lego(inbuf, got);
00368             if (msg>=0) {   
00369 //              printf("parsed message %d\n",msg);
00370                 c_msg = (u_char)msg;
00371                 btn_led_add_pattern(BTN_LED_PATTERN_KNIGHT, 0, 1, 1);               
00372                 _send_to_slave(&c_msg,1);
00373                 printf("parsed message %d\n",msg);
00374             }
00375         }
00376     }
00377 } 
00378 
00379  
00380 int main(void){   
00381     _init_btn();
00382     _init_duart();
00383     _init_suart();
00384     printf_P(PSTR("\n\nHello, this is bt-lego\n"));
00385     printf_P(PSTR("Initializing BT stacks ..."));
00386     _init_bt();
00387     printf_P(PSTR("ok\n"));
00388     if (_am_i_master()) {
00389         printf_P(PSTR("I am the master\n"));
00390         NutThreadCreate("T_Master", lego_master, 0, 512);           
00391     }
00392     else {
00393         printf_P(PSTR("I am the slave\n"));
00394         NutThreadCreate("T_Slave", lego_slave, 0, 192);    
00395     }
00396 
00397     _run_terminal();
00398     return 1;
00399 }