git.gag.com Git - fw/altos/blob - src/ao_radio_cmac.c

   1 /*
   2  * Copyright © 2011 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #include "ao.h"
  19
  20 #define AO_CMAC_KEY_LEN         AO_AES_LEN
  21 #define AO_CMAC_MAX_LEN         (128 - AO_CMAC_KEY_LEN)
  22
  23 static __xdata uint8_t ao_radio_cmac_mutex;
  24 __pdata int16_t ao_radio_cmac_rssi;
  25 static __xdata uint8_t cmac_data[AO_CMAC_MAX_LEN + AO_CMAC_KEY_LEN + 2 + AO_CMAC_KEY_LEN];
  26 static __pdata uint8_t ao_radio_cmac_len;
  27
  28 static uint8_t
  29 getnibble(void)
  30 {
  31         int8_t  b;
  32
  33         b = ao_cmd_hexchar(getchar());
  34         if (b < 0) {
  35                 ao_cmd_status = ao_cmd_lex_error;
  36                 return 0;
  37         }
  38         return (uint8_t) b;
  39 }
  40
  41 static uint8_t
  42 getbyte(void)
  43 {
  44         uint8_t b;
  45         b = getnibble() << 4;
  46         b |= getnibble();
  47         return b;
  48 }
  49
  50 static uint8_t
  51 round_len(uint8_t len)
  52 {
  53         uint8_t rem;
  54
  55         /* Make sure we transfer at least one packet, and
  56          * then make sure every packet is full. Note that
  57          * there is no length encoded, and that the receiver
  58          * must deal with any extra bytes in the packet
  59          */
  60         if (len < AO_CMAC_KEY_LEN)
  61                 len = AO_CMAC_KEY_LEN;
  62         rem = len % AO_CMAC_KEY_LEN;
  63         if (rem != 0)
  64                 len += (AO_CMAC_KEY_LEN - rem);
  65         return len;
  66 }
  67
  68 /*
  69  * Sign and deliver the data sitting in the cmac buffer
  70  */
  71 static void
  72 radio_cmac_send(uint8_t len) __reentrant
  73 {
  74         uint8_t i;
  75
  76         len = round_len(len);
  77         /* Make sure the AES key is loaded */
  78         ao_config_get();
  79
  80 #if HAS_MONITOR
  81         ao_set_monitor(0);
  82 #endif
  83
  84         ao_mutex_get(&ao_aes_mutex);
  85         ao_aes_set_mode(ao_aes_mode_cbc_mac);
  86         ao_aes_set_key(ao_config.aes_key);
  87         ao_aes_zero_iv();
  88         for (i = 0; i < len; i += AO_CMAC_KEY_LEN) {
  89                 if (i + AO_CMAC_KEY_LEN < len)
  90                         ao_aes_run(&cmac_data[i], NULL);
  91                 else
  92                         ao_aes_run(&cmac_data[i], &cmac_data[len]);
  93         }
  94         ao_mutex_put(&ao_aes_mutex);
  95
  96         ao_radio_send(cmac_data, len + AO_CMAC_KEY_LEN);
  97 }
  98
  99 /*
 100  * Receive and validate an incoming packet
 101  */
 102
 103 static int8_t
 104 radio_cmac_recv(uint8_t len, uint16_t timeout) __reentrant
 105 {
 106         uint8_t i;
 107
 108         len = round_len(len);
 109 #if HAS_MONITOR
 110         ao_set_monitor(0);
 111 #endif
 112         if (timeout)
 113                 ao_alarm(timeout);
 114
 115         i = ao_radio_recv(cmac_data, len + AO_CMAC_KEY_LEN + 2);
 116         ao_clear_alarm();
 117
 118         if (!i) {
 119                 ao_radio_cmac_rssi = 0;
 120                 return AO_RADIO_CMAC_TIMEOUT;
 121         }
 122
 123         ao_radio_cmac_rssi = (int16_t) (((int8_t) cmac_data[len + AO_CMAC_KEY_LEN]) >> 1) - 74;
 124         if (!(cmac_data[len + AO_CMAC_KEY_LEN +1] & PKT_APPEND_STATUS_1_CRC_OK))
 125                 return AO_RADIO_CMAC_CRC_ERROR;
 126
 127         ao_config_get();
 128
 129         /* Compute the packet signature
 130          */
 131         ao_mutex_get(&ao_aes_mutex);
 132         ao_aes_set_mode(ao_aes_mode_cbc_mac);
 133         ao_aes_set_key(ao_config.aes_key);
 134         ao_aes_zero_iv();
 135         for (i = 0; i < len; i += AO_CMAC_KEY_LEN) {
 136                 if (i + AO_CMAC_KEY_LEN < len)
 137                         ao_aes_run(&cmac_data[i], NULL);
 138                 else
 139                         ao_aes_run(&cmac_data[i], &cmac_data[len + AO_CMAC_KEY_LEN + 2]);
 140         }
 141         ao_mutex_put(&ao_aes_mutex);
 142
 143         /* Check the packet signature against the signature provided
 144          * over the link
 145          */
 146
 147         if (memcmp(&cmac_data[len],
 148                    &cmac_data[len + AO_CMAC_KEY_LEN + 2],
 149                    AO_CMAC_KEY_LEN) != 0) {
 150                 return AO_RADIO_CMAC_MAC_ERROR;
 151         }
 152
 153         return AO_RADIO_CMAC_OK;
 154 }
 155
 156 int8_t
 157 ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant
 158 {
 159         if (len > AO_CMAC_MAX_LEN)
 160                 return AO_RADIO_CMAC_LEN_ERROR;
 161         ao_mutex_get(&ao_radio_cmac_mutex);
 162         memcpy(cmac_data, packet, len);
 163         radio_cmac_send(len);
 164         ao_mutex_put(&ao_radio_cmac_mutex);
 165         return AO_RADIO_CMAC_OK;
 166 }
 167
 168 int8_t
 169 ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant
 170 {
 171         uint8_t i;
 172         if (len > AO_CMAC_MAX_LEN)
 173                 return AO_RADIO_CMAC_LEN_ERROR;
 174         ao_mutex_get(&ao_radio_cmac_mutex);
 175         i = radio_cmac_recv(len, timeout);
 176         if (i == AO_RADIO_CMAC_OK)
 177                 memcpy(packet, cmac_data, len);
 178         ao_mutex_put(&ao_radio_cmac_mutex);
 179         return i;
 180 }
 181
 182 static void
 183 radio_cmac_send_cmd(void) __reentrant
 184 {
 185         uint8_t i;
 186         uint8_t len;
 187
 188         ao_cmd_decimal();
 189         if (ao_cmd_status != ao_cmd_success)
 190                 return;
 191         len = ao_cmd_lex_i;
 192         if (len > AO_CMAC_MAX_LEN) {
 193                 ao_cmd_status = ao_cmd_syntax_error;
 194                 return;
 195         }
 196         flush();
 197         ao_mutex_get(&ao_radio_cmac_mutex);
 198         len = ao_cmd_lex_i;
 199         for (i = 0; i < len; i++) {
 200                 cmac_data[i] = getbyte();
 201                 if (ao_cmd_status != ao_cmd_success)
 202                         return;
 203         }
 204         radio_cmac_send(len);
 205         ao_mutex_put(&ao_radio_cmac_mutex);
 206 }
 207
 208 static void
 209 radio_cmac_recv_cmd(void) __reentrant
 210 {
 211         uint8_t         len, i;
 212         uint16_t        timeout;
 213
 214         ao_cmd_decimal();
 215         if (ao_cmd_status != ao_cmd_success)
 216                 return;
 217         len = ao_cmd_lex_i;
 218         ao_cmd_decimal();
 219         if (ao_cmd_status != ao_cmd_success)
 220                 return;
 221         timeout = AO_MS_TO_TICKS(ao_cmd_lex_i);
 222         ao_mutex_get(&ao_radio_cmac_mutex);
 223         i = radio_cmac_recv(len, timeout);
 224         if (i == AO_RADIO_CMAC_OK) {
 225                 printf ("PACKET ");
 226                 for (i = 0; i < len; i++)
 227                         printf("%02x", cmac_data[i]);
 228                 printf (" %d\n", ao_radio_cmac_rssi);
 229         } else
 230                 printf ("ERROR %d %d\n", i, ao_radio_cmac_rssi);
 231         ao_mutex_put(&ao_radio_cmac_mutex);
 232 }
 233
 234 static __xdata struct ao_launch_command command;
 235 static __xdata struct ao_launch_query   query;
 236 static pdata uint16_t   launch_serial;
 237 static pdata uint8_t    launch_channel;
 238 static pdata uint16_t   tick_offset;
 239
 240 static void
 241 launch_args(void) __reentrant
 242 {
 243         ao_cmd_decimal();
 244         launch_serial = ao_cmd_lex_i;
 245         ao_cmd_decimal();
 246         launch_channel = ao_cmd_lex_i;
 247 }
 248
 249 static int8_t
 250 launch_query(void)
 251 {
 252         uint8_t i;
 253         int8_t  r = AO_RADIO_CMAC_OK;
 254
 255         tick_offset = ao_time();
 256         for (i = 0; i < 10; i++) {
 257                 printf ("."); flush();
 258                 command.tick = ao_time();
 259                 command.serial = launch_serial;
 260                 command.cmd = AO_LAUNCH_QUERY;
 261                 command.channel = launch_channel;
 262                 ao_radio_cmac_send(&command, sizeof (command));
 263                 r = ao_radio_cmac_recv(&query, sizeof (query), AO_MS_TO_TICKS(500));
 264                 if (r == AO_RADIO_CMAC_OK)
 265                         break;
 266         }
 267         tick_offset -= query.tick;
 268         printf("\n"); flush();
 269         return r;
 270 }
 271
 272 static void
 273 launch_report_cmd(void) __reentrant
 274 {
 275         int8_t          r;
 276
 277         launch_args();
 278         if (ao_cmd_status != ao_cmd_success)
 279                 return;
 280         r = launch_query();
 281         switch (r) {
 282         case AO_RADIO_CMAC_OK:
 283                 if (query.valid) {
 284                         switch (query.arm_status) {
 285                         case ao_igniter_ready:
 286                         case ao_igniter_active:
 287                                 printf ("Armed: ");
 288                                 break;
 289                         default:
 290                                 printf("Disarmed: ");
 291                         }
 292                         switch (query.igniter_status) {
 293                         default:
 294                                 printf("unknown\n");
 295                                 break;
 296                         case ao_igniter_ready:
 297                                 printf("igniter good\n");
 298                                 break;
 299                         case ao_igniter_open:
 300                                 printf("igniter bad\n");
 301                                 break;
 302                         }
 303                 } else {
 304                         printf("Invalid channel %d\n", launch_channel);
 305                 }
 306                 printf("Rssi: %d\n", ao_radio_cmac_rssi);
 307                 break;
 308         default:
 309                 printf("Error %d\n", r);
 310                 break;
 311         }
 312 }
 313
 314 static void
 315 launch_arm(void) __reentrant
 316 {
 317         command.tick = ao_time() - tick_offset;
 318         command.serial = launch_serial;
 319         command.cmd = AO_LAUNCH_ARM;
 320         command.channel = launch_channel;
 321         ao_radio_cmac_send(&command, sizeof (command));
 322 }
 323
 324 static void
 325 launch_ignite(void) __reentrant
 326 {
 327         command.tick = ao_time() - tick_offset;
 328         command.serial = launch_serial;
 329         command.cmd = AO_LAUNCH_FIRE;
 330         command.channel = 0;
 331         ao_radio_cmac_send(&command, sizeof (command));
 332 }
 333
 334 static void
 335 launch_fire_cmd(void) __reentrant
 336 {
 337         static __xdata struct ao_launch_command command;
 338         uint8_t         secs;
 339         uint8_t         i;
 340         int8_t          r;
 341
 342         launch_args();
 343         ao_cmd_decimal();
 344         secs = ao_cmd_lex_i;
 345         if (ao_cmd_status != ao_cmd_success)
 346                 return;
 347         r = launch_query();
 348         if (r != AO_RADIO_CMAC_OK) {
 349                 printf("query failed %d\n", r);
 350                 return;
 351         }
 352
 353         for (i = 0; i < 4; i++) {
 354                 printf("arm %d\n", i); flush();
 355                 launch_arm();
 356         }
 357
 358         secs = secs * 10 - 5;
 359         if (secs > 100)
 360                 secs = 100;
 361         for (i = 0; i < secs; i++) {
 362                 printf("fire %d\n", i); flush();
 363                 launch_ignite();
 364                 ao_delay(AO_MS_TO_TICKS(100));
 365         }
 366 }
 367
 368 static void
 369 launch_arm_cmd(void) __reentrant
 370 {
 371         uint8_t i;
 372         int8_t  r;
 373         launch_args();
 374         r = launch_query();
 375         if (r != AO_RADIO_CMAC_OK) {
 376                 printf("query failed %d\n", r);
 377                 return;
 378         }
 379         for (i = 0; i < 4; i++)
 380                 launch_arm();
 381 }
 382
 383 static void
 384 launch_ignite_cmd(void) __reentrant
 385 {
 386         uint8_t i;
 387         launch_args();
 388         for (i = 0; i < 4; i++)
 389                 launch_ignite();
 390 }
 391
 392 static __code struct ao_cmds ao_radio_cmac_cmds[] = {
 393         { radio_cmac_send_cmd,  "s <length>\0Send AES-CMAC packet. Bytes to send follow on next line" },
 394         { radio_cmac_recv_cmd,  "S <length> <timeout>\0Receive AES-CMAC packet. Timeout in ms" },
 395         { launch_report_cmd,    "l <serial> <channel>\0Get remote launch status" },
 396         { launch_fire_cmd,      "f <serial> <channel> <secs>\0Fire remote igniter" },
 397         { launch_arm_cmd,       "a <serial> <channel>\0Arm remote igniter" },
 398         { launch_ignite_cmd,    "i <serial> <channel>\0Pulse remote igniter" },
 399         { 0, NULL },
 400 };
 401
 402 void
 403 ao_radio_cmac_init(void)
 404 {
 405         ao_cmd_register(&ao_radio_cmac_cmds[0]);
 406 }