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 static __xdata uint8_t cmac_data[AO_CMAC_MAX_LEN + AO_CMAC_KEY_LEN + 2 + AO_CMAC_KEY_LEN];
  25 static __pdata uint8_t ao_radio_cmac_len;
  26
  27 static uint8_t
  28 getnibble(void)
  29 {
  30         int8_t  b;
  31
  32         b = ao_cmd_hexchar(getchar());
  33         if (b < 0) {
  34                 ao_cmd_status = ao_cmd_lex_error;
  35                 return 0;
  36         }
  37         return (uint8_t) b;
  38 }
  39
  40 static uint8_t
  41 getbyte(void)
  42 {
  43         uint8_t b;
  44         b = getnibble() << 4;
  45         b |= getnibble();
  46         return b;
  47 }
  48
  49 static uint8_t
  50 round_len(uint8_t len)
  51 {
  52         uint8_t rem;
  53
  54         /* Make sure we transfer at least one packet, and
  55          * then make sure every packet is full. Note that
  56          * there is no length encoded, and that the receiver
  57          * must deal with any extra bytes in the packet
  58          */
  59         if (len < AO_CMAC_KEY_LEN)
  60                 len = AO_CMAC_KEY_LEN;
  61         rem = len % AO_CMAC_KEY_LEN;
  62         if (rem != 0)
  63                 len += (AO_CMAC_KEY_LEN - rem);
  64         return len;
  65 }
  66
  67 /*
  68  * Sign and deliver the data sitting in the cmac buffer
  69  */
  70 static void
  71 radio_cmac_send(uint8_t len) __reentrant
  72 {
  73         uint8_t i;
  74
  75         len = round_len(len);
  76         /* Make sure the AES key is loaded */
  77         ao_config_get();
  78
  79 #if HAS_MONITOR
  80         ao_set_monitor(0);
  81 #endif
  82
  83         ao_mutex_get(&ao_aes_mutex);
  84         ao_aes_set_mode(ao_aes_mode_cbc_mac);
  85         ao_aes_set_key(ao_config.aes_key);
  86         ao_aes_zero_iv();
  87         for (i = 0; i < len; i += AO_CMAC_KEY_LEN) {
  88                 if (i + AO_CMAC_KEY_LEN < len)
  89                         ao_aes_run(&cmac_data[i], NULL);
  90                 else
  91                         ao_aes_run(&cmac_data[i], &cmac_data[len]);
  92         }
  93         ao_mutex_put(&ao_aes_mutex);
  94
  95         ao_radio_send(cmac_data, len + AO_CMAC_KEY_LEN);
  96 }
  97
  98 /*
  99  * Receive and validate an incoming packet
 100  */
 101
 102 static int8_t
 103 radio_cmac_recv(uint8_t len, uint16_t timeout) __reentrant
 104 {
 105         uint8_t i;
 106
 107         len = round_len(len);
 108 #if HAS_MONITOR
 109         ao_set_monitor(0);
 110 #endif
 111         if (timeout)
 112                 ao_alarm(timeout);
 113
 114         i = ao_radio_recv(cmac_data, len + AO_CMAC_KEY_LEN + 2);
 115         ao_clear_alarm();
 116
 117         if (!i)
 118                 return AO_RADIO_CMAC_TIMEOUT;
 119
 120         if (!(cmac_data[len + AO_CMAC_KEY_LEN +1] & PKT_APPEND_STATUS_1_CRC_OK))
 121                 return AO_RADIO_CMAC_CRC_ERROR;
 122
 123         ao_config_get();
 124
 125         /* Compute the packet signature
 126          */
 127         ao_mutex_get(&ao_aes_mutex);
 128         ao_aes_set_mode(ao_aes_mode_cbc_mac);
 129         ao_aes_set_key(ao_config.aes_key);
 130         ao_aes_zero_iv();
 131         for (i = 0; i < len; i += AO_CMAC_KEY_LEN) {
 132                 if (i + AO_CMAC_KEY_LEN < len)
 133                         ao_aes_run(&cmac_data[i], NULL);
 134                 else
 135                         ao_aes_run(&cmac_data[i], &cmac_data[len + AO_CMAC_KEY_LEN + 2]);
 136         }
 137         ao_mutex_put(&ao_aes_mutex);
 138
 139         /* Check the packet signature against the signature provided
 140          * over the link
 141          */
 142
 143         if (memcmp(&cmac_data[len],
 144                    &cmac_data[len + AO_CMAC_KEY_LEN + 2],
 145                    AO_CMAC_KEY_LEN) != 0) {
 146                 return AO_RADIO_CMAC_MAC_ERROR;
 147         }
 148
 149         return AO_RADIO_CMAC_OK;
 150 }
 151
 152 int8_t
 153 ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant
 154 {
 155         if (len > AO_CMAC_MAX_LEN)
 156                 return AO_RADIO_CMAC_LEN_ERROR;
 157         ao_mutex_get(&ao_radio_cmac_mutex);
 158         memcpy(cmac_data, packet, len);
 159         radio_cmac_send(len);
 160         ao_mutex_put(&ao_radio_cmac_mutex);
 161         return AO_RADIO_CMAC_OK;
 162 }
 163
 164 int8_t
 165 ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant
 166 {
 167         uint8_t i;
 168         if (len > AO_CMAC_MAX_LEN)
 169                 return AO_RADIO_CMAC_LEN_ERROR;
 170         ao_mutex_get(&ao_radio_cmac_mutex);
 171         i = radio_cmac_recv(len, timeout);
 172         if (i == AO_RADIO_CMAC_OK)
 173                 memcpy(packet, cmac_data, len);
 174         ao_mutex_put(&ao_radio_cmac_mutex);
 175         return i;
 176 }
 177
 178 static void
 179 radio_cmac_send_cmd(void) __reentrant
 180 {
 181         uint8_t i;
 182         uint8_t len;
 183
 184         ao_cmd_decimal();
 185         if (ao_cmd_status != ao_cmd_success)
 186                 return;
 187         len = ao_cmd_lex_i;
 188         if (len > AO_CMAC_MAX_LEN) {
 189                 ao_cmd_status = ao_cmd_syntax_error;
 190                 return;
 191         }
 192         flush();
 193         ao_mutex_get(&ao_radio_cmac_mutex);
 194         len = ao_cmd_lex_i;
 195         for (i = 0; i < len; i++) {
 196                 cmac_data[i] = getbyte();
 197                 if (ao_cmd_status != ao_cmd_success)
 198                         return;
 199         }
 200         radio_cmac_send(len);
 201         ao_mutex_put(&ao_radio_cmac_mutex);
 202 }
 203
 204 static void
 205 radio_cmac_recv_cmd(void) __reentrant
 206 {
 207         uint8_t         len, i;
 208         uint16_t        timeout;
 209
 210         ao_cmd_decimal();
 211         if (ao_cmd_status != ao_cmd_success)
 212                 return;
 213         len = ao_cmd_lex_i;
 214         ao_cmd_decimal();
 215         if (ao_cmd_status != ao_cmd_success)
 216                 return;
 217         timeout = AO_MS_TO_TICKS(ao_cmd_lex_i);
 218         ao_mutex_get(&ao_radio_cmac_mutex);
 219         i = radio_cmac_recv(len, timeout);
 220         if (i == AO_RADIO_CMAC_OK) {
 221                 printf ("PACKET ");
 222                 for (i = 0; i < len; i++)
 223                         printf("%02x", cmac_data[i]);
 224                 printf ("\n");
 225         } else
 226                 printf ("ERROR %d\n", i);
 227         ao_mutex_put(&ao_radio_cmac_mutex);
 228 }
 229
 230 static __xdata struct ao_launch_command command;
 231 static __xdata struct ao_launch_query   query;
 232
 233
 234 static int8_t
 235 launch_query(uint16_t serial, uint8_t channel)
 236 {
 237         uint8_t i;
 238         int8_t  r = AO_RADIO_CMAC_OK;
 239
 240         for (i = 0; i < 10; i++) {
 241                 printf ("."); flush();
 242                 command.tick = ao_time();
 243                 command.serial = serial;
 244                 command.cmd = AO_LAUNCH_QUERY;
 245                 command.channel = channel;
 246                 ao_radio_cmac_send(&command, sizeof (command));
 247                 r = ao_radio_cmac_recv(&query, sizeof (query), AO_MS_TO_TICKS(500));
 248                 if (r == AO_RADIO_CMAC_OK)
 249                         break;
 250         }
 251         printf("\n"); flush();
 252         return r;
 253 }
 254
 255 static void
 256 launch_report_cmd(void) __reentrant
 257 {
 258         uint8_t         channel;
 259         uint16_t        serial;
 260         int8_t          r;
 261
 262         ao_cmd_decimal();
 263         serial = ao_cmd_lex_i;
 264         ao_cmd_decimal();
 265         channel = ao_cmd_lex_i;
 266         if (ao_cmd_status != ao_cmd_success)
 267                 return;
 268         r = launch_query(serial, channel);
 269         switch (r) {
 270         case AO_RADIO_CMAC_OK:
 271                 if (query.valid) {
 272                         switch (query.arm_status) {
 273                         case ao_igniter_ready:
 274                         case ao_igniter_active:
 275                                 printf ("Armed: ");
 276                                 switch (query.igniter_status) {
 277                                 default:
 278                                         printf("unknown status\n");
 279                                         break;
 280                                 case ao_igniter_ready:
 281                                         printf("igniter good\n");
 282                                         break;
 283                                 case ao_igniter_open:
 284                                         printf("igniter bad\n");
 285                                         break;
 286                                 }
 287                                 break;
 288                         default:
 289                                 printf("Disarmed\n");
 290                         }
 291                 } else {
 292                         printf("Invalid channel %d\n", channel);
 293                 }
 294                 break;
 295         default:
 296                 printf("Error %d\n", r);
 297                 break;
 298         }
 299 }
 300
 301 static void
 302 launch_fire_cmd(void) __reentrant
 303 {
 304         static __xdata struct ao_launch_command command;
 305         uint8_t         channel;
 306         uint16_t        serial;
 307         uint8_t         secs;
 308         uint8_t         i;
 309         int8_t          r;
 310         uint16_t        tick_offset;
 311
 312         ao_cmd_decimal();
 313         serial = ao_cmd_lex_i;
 314         ao_cmd_decimal();
 315         channel = ao_cmd_lex_i;
 316         ao_cmd_decimal();
 317         secs = ao_cmd_lex_i;
 318         if (ao_cmd_status != ao_cmd_success)
 319                 return;
 320         tick_offset = ao_time();
 321         r = launch_query(serial, channel);
 322         tick_offset -= query.tick;
 323
 324         for (i = 0; i < 4; i++) {
 325                 printf("arm %d\n", i); flush();
 326                 command.tick = ao_time() - tick_offset;
 327                 command.serial = serial;
 328                 command.cmd = AO_LAUNCH_ARM;
 329                 command.channel = channel;
 330                 ao_radio_cmac_send(&command, sizeof (command));
 331         }
 332         secs = secs * 10 - 5;
 333         if (secs > 100)
 334                 secs = 100;
 335         for (i = 0; i < secs; i++) {
 336                 printf("fire %d\n", i); flush();
 337                 command.tick = ao_time() - tick_offset;
 338                 command.serial = serial;
 339                 command.cmd = AO_LAUNCH_FIRE;
 340                 command.channel = 0;
 341                 ao_radio_cmac_send(&command, sizeof (command));
 342                 ao_delay(AO_MS_TO_TICKS(100));
 343         }
 344 }
 345
 346 static __code struct ao_cmds ao_radio_cmac_cmds[] = {
 347         { radio_cmac_send_cmd,  "s <length>\0Send AES-CMAC packet. Bytes to send follow on next line" },
 348         { radio_cmac_recv_cmd,  "S <length> <timeout>\0Receive AES-CMAC packet. Timeout in ms" },
 349         { launch_report_cmd,    "l <serial> <channel>\0Get remote launch status" },
 350         { launch_fire_cmd,      "f <serial> <channel> <secs>\0Fire remote igniter" },
 351         { 0, NULL },
 352 };
 353
 354 void
 355 ao_radio_cmac_init(void)
 356 {
 357         ao_cmd_register(&ao_radio_cmac_cmds[0]);
 358 }