diff options
Diffstat (limited to 'r2d2.c')
| -rw-r--r-- | r2d2.c | 264 |
1 files changed, 264 insertions, 0 deletions
@@ -0,0 +1,264 @@ +/* + r2d2 - interact with Sphero R2-D2 + Copyright (C) 2020 Ian Cowburn + + 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 3 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, see <http://www.gnu.org/licenses/>. +*/ +#include <stdlib.h> +#include <stdio.h> +#include <string.h> +#include <errno.h> + +#include <unistd.h> +#include <sys/socket.h> +#include <bluetooth/bluetooth.h> +#include <bluetooth/hci.h> +#include <bluetooth/hci_lib.h> +#include <bluetooth/l2cap.h> + +typedef struct +{ + const char *name; + int command[20]; +} Command; + +static Command plain_commands[] = +{ + {"laugh", {0x0A,0x18,0x00,0x1F,0x00,0x32,0x00,0x00,0x00,0x00,0x00,-1}}, + {"yes", {0x0A,0x17,0x05,0x41,0x00,0x0F,-1}}, + {"no", {0x0A,0x17,0x05,0x3F,0x00,0x10,-1}}, + {"alarm", {0x0A,0x17,0x05,0x17,0x00,0x07,-1}}, + {"angry", {0x0A,0x17,0x05,0x18,0x00,0x08,-1}}, + {"annoyed", {0x0A,0x17,0x05,0x19,0x00,0x09,-1}}, + {"ionblast", {0x0A,0x17,0x05,0x1A,0x00,0x0E,-1}}, + {"sad", {0x0A,0x17,0x05,0x1C,0x00,0x11,-1}}, + {"scared", {0x0A,0x17,0x05,0x1D,0x00,0x13,-1}}, + {"chatty", {0x0A,0x17,0x05,0x17,0x00,0x0A,-1}}, + {"confident", {0x0A,0x17,0x05,0x18,0x00,0x12,-1}}, + {"excited", {0x0A,0x17,0x05,0x19,0x00,0x0C,-1}}, + {"happy", {0x0A,0x17,0x05,0x1A,0x00,0x0D,-1}}, + {"laugh", {0x0A,0x17,0x05,0x1B,0x00,0x0F,-1}}, + {"surprise", {0x0A,0x17,0x05,0x1C,0x00,0x18,-1}}, + {"tripod", {0x0A,0x17,0x0D,0x1D,0x01,-1}}, + {"bipod", {0x0A,0x17,0x0D,0x1C,0x02,-1}}, + {NULL, {0}} +}; + +static unsigned char force_command[] = +{ + 0x75,0x73,0x65,0x74,0x68,0x65,0x66,0x6F,0x72,0x63,0x65,0x2E, + 0x2E,0x2E,0x62,0x61,0x6E,0x64 +}; + +static Command wake_command = +{ + "wakeup", + { + 0x0A,0x13,0x0D,0x00,-1 + } +}; + +static void Fatal(const char *call) +{ + perror(call); + exit(EXIT_FAILURE); +} + +static unsigned char Checksum(const char *p, int len) +{ + unsigned char c; + int f; + + c = 0; + + for(f = 0; f < len; f++) + { + c += p[f]; + } + + c = ~c; + + return c; +} + +static int Write(int sock, unsigned char *buff, int len) +{ + int wr = 0; + int tot = 0; + + while(tot < len) + { + wr = write(sock, buff + tot, len - tot); + + if (wr < 1) + { + return -1; + } + + tot += wr; + } + + return len; +} + +static void CharWriteHandle(int sock, int handle, + const unsigned char *cmd, int len) +{ + unsigned char buff[1024]; + unsigned short *us; + + memcpy(buff + 3, cmd, len); + len += 3; + + buff[0] = 0x12; + + us = (unsigned short *)(buff + 1); + *us = htobs(handle); + + if (Write(sock, buff, len) == -1) + { + Fatal("write"); + } +} + +static void SendCommand(int sock, int handle, const Command *cmd) +{ + unsigned char buff[1024]; + unsigned short *us; + int len; + int f; + + for(f = 0; cmd->command[f] != -1; f++) + { + buff[f] = (unsigned char)cmd->command[f]; + } + + len = f; + + buff[len] = Checksum(buff, len); + len++; + + memmove(buff + 1, buff, len++); + buff[0] = 0x8du; + buff[++len] = 0xd8u; + + CharWriteHandle(sock, handle, buff, len); +} + +int main(int argc, char *argv[]) +{ + char address[32] = {0}; + struct sockaddr_l2 src_addr = {0}; + struct sockaddr_l2 dest_addr = {0}; + int base; + int f; + int sock; + + /* Get address + */ + if (getenv("R2D2")) + { + snprintf(address, sizeof address, "%s", getenv("R2D2")); + } + + if (argc > 2 && strcmp(argv[1], "-a") == 0) + { + snprintf(address, sizeof address, "%s", argv[2]); + base = 3; + } + else + { + base = 1; + } + + /* Check address and arguments supplied + */ + if (!address[0]) + { + fprintf(stderr, "%s: pass address or set R2D2 variable\n", argv[0]); + return EXIT_FAILURE; + } + + if (argc <= base) + { + fprintf(stderr, "%s: usage %s [-a address] command [..command]\n", + argv[0], argv[0]); + return EXIT_FAILURE; + } + + /* Open connection to R2 + */ + sock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP); + + if (sock == -1) + { + Fatal("socket"); + } + + src_addr.l2_family = AF_BLUETOOTH; + src_addr.l2_cid = htobs(4); + bacpy(&src_addr.l2_bdaddr, BDADDR_ANY); + src_addr.l2_bdaddr_type = BDADDR_LE_RANDOM; + + if (bind(sock, (void *)&src_addr, sizeof(src_addr)) == -1) + { + Fatal("bind"); + } + + dest_addr.l2_family = AF_BLUETOOTH; + dest_addr.l2_cid = htobs(4); + str2ba(address, &dest_addr.l2_bdaddr); + dest_addr.l2_bdaddr_type = BDADDR_LE_RANDOM; + + if (connect(sock, (void *)&dest_addr, sizeof(dest_addr)) == -1) + { + Fatal("connect"); + } + + CharWriteHandle(sock, 0x15, force_command, sizeof force_command); + SendCommand(sock, 0x1c, &wake_command); + + for(f = base; f < argc; f++) + { + int n; + + Command *cmd = NULL; + + for(n = 0; plain_commands[n].name && !cmd; n++) + { + if (strcmp(argv[f], plain_commands[n].name) == 0) + { + cmd = plain_commands + n; + } + } + + if (cmd == NULL) + { + fprintf(stderr, "%s: unknown command %s\n", argv[0], argv[f]); + } + else + { + SendCommand(sock, 0x1c, cmd); + } + } + + /* All done + */ + if (close(sock) == -1) + { + Fatal("close"); + } + + return EXIT_SUCCESS; +} |