git.gag.com Git - fw/altos/blob - src/core/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_monitor_set(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_monitor_set(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] & AO_RADIO_STATUS_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         ao_xmemcpy(cmac_data, packet, len);
 163 #if AO_LED_TX
 164         ao_led_on(AO_LED_TX);
 165 #endif
 166         radio_cmac_send(len);
 167 #if AO_LED_TX
 168         ao_led_off(AO_LED_TX);
 169 #endif
 170         ao_mutex_put(&ao_radio_cmac_mutex);
 171         return AO_RADIO_CMAC_OK;
 172 }
 173
 174 int8_t
 175 ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant
 176 {
 177         uint8_t i;
 178         if (len > AO_CMAC_MAX_LEN)
 179                 return AO_RADIO_CMAC_LEN_ERROR;
 180         ao_mutex_get(&ao_radio_cmac_mutex);
 181 #if AO_LED_RX
 182         ao_led_on(AO_LED_RX);
 183 #endif
 184         i = radio_cmac_recv(len, timeout);
 185 #if AO_LED_RX
 186         ao_led_off(AO_LED_RX);
 187 #endif
 188         if (i == AO_RADIO_CMAC_OK)
 189                 ao_xmemcpy(packet, cmac_data, len);
 190         ao_mutex_put(&ao_radio_cmac_mutex);
 191         return i;
 192 }
 193
 194 static void
 195 radio_cmac_send_cmd(void) __reentrant
 196 {
 197         uint8_t i;
 198         uint8_t len;
 199
 200         ao_cmd_decimal();
 201         if (ao_cmd_status != ao_cmd_success)
 202                 return;
 203         len = ao_cmd_lex_i;
 204         if (len > AO_CMAC_MAX_LEN) {
 205                 ao_cmd_status = ao_cmd_syntax_error;
 206                 return;
 207         }
 208         flush();
 209         ao_mutex_get(&ao_radio_cmac_mutex);
 210         len = ao_cmd_lex_i;
 211         for (i = 0; i < len; i++) {
 212                 cmac_data[i] = getbyte();
 213                 if (ao_cmd_status != ao_cmd_success)
 214                         return;
 215         }
 216         radio_cmac_send(len);
 217         ao_mutex_put(&ao_radio_cmac_mutex);
 218 }
 219
 220 static void
 221 radio_cmac_recv_cmd(void) __reentrant
 222 {
 223         uint8_t         len, i;
 224         uint16_t        timeout;
 225
 226         ao_cmd_decimal();
 227         if (ao_cmd_status != ao_cmd_success)
 228                 return;
 229         len = ao_cmd_lex_i;
 230         ao_cmd_decimal();
 231         if (ao_cmd_status != ao_cmd_success)
 232                 return;
 233         timeout = AO_MS_TO_TICKS(ao_cmd_lex_i);
 234         ao_mutex_get(&ao_radio_cmac_mutex);
 235         i = radio_cmac_recv(len, timeout);
 236         if (i == AO_RADIO_CMAC_OK) {
 237                 printf ("PACKET ");
 238                 for (i = 0; i < len; i++)
 239                         printf("%02x", cmac_data[i]);
 240                 printf (" %d\n", ao_radio_cmac_rssi);
 241         } else
 242                 printf ("ERROR %d %d\n", i, ao_radio_cmac_rssi);
 243         ao_mutex_put(&ao_radio_cmac_mutex);
 244 }
 245