quantum/serial_link/protocol/frame_validator.c

   1 /*
   2 The MIT License (MIT)
   3
   4 Copyright (c) 2016 Fred Sundvik
   5
   6 Permission is hereby granted, free of charge, to any person obtaining a copy
   7 of this software and associated documentation files (the "Software"), to deal
   8 in the Software without restriction, including without limitation the rights
   9 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  10 copies of the Software, and to permit persons to whom the Software is
  11 furnished to do so, subject to the following conditions:
  12
  13 The above copyright notice and this permission notice shall be included in all
  14 copies or substantial portions of the Software.
  15
  16 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  17 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  18 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  19 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  20 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  21 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  22 SOFTWARE.
  23 */
  24
  25 #include "serial_link/protocol/frame_validator.h"
  26 #include "serial_link/protocol/frame_router.h"
  27 #include "serial_link/protocol/byte_stuffer.h"
  28 #include <string.h>
  29
  30 const uint32_t poly8_lookup[256] =
  31 {
  32  0, 0x77073096, 0xEE0E612C, 0x990951BA,
  33  0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3,
  34  0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988,
  35  0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, 0x90BF1D91,
  36  0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE,
  37  0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7,
  38  0x136C9856, 0x646BA8C0, 0xFD62F97A, 0x8A65C9EC,
  39  0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5,
  40  0x3B6E20C8, 0x4C69105E, 0xD56041E4, 0xA2677172,
  41  0x3C03E4D1, 0x4B04D447, 0xD20D85FD, 0xA50AB56B,
  42  0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940,
  43  0x32D86CE3, 0x45DF5C75, 0xDCD60DCF, 0xABD13D59,
  44  0x26D930AC, 0x51DE003A, 0xC8D75180, 0xBFD06116,
  45  0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F,
  46  0x2802B89E, 0x5F058808, 0xC60CD9B2, 0xB10BE924,
  47  0x2F6F7C87, 0x58684C11, 0xC1611DAB, 0xB6662D3D,
  48  0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A,
  49  0x71B18589, 0x06B6B51F, 0x9FBFE4A5, 0xE8B8D433,
  50  0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818,
  51  0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01,
  52  0x6B6B51F4, 0x1C6C6162, 0x856530D8, 0xF262004E,
  53  0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457,
  54  0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C,
  55  0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3, 0xFBD44C65,
  56  0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2,
  57  0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB,
  58  0x4369E96A, 0x346ED9FC, 0xAD678846, 0xDA60B8D0,
  59  0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9,
  60  0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086,
  61  0x5768B525, 0x206F85B3, 0xB966D409, 0xCE61E49F,
  62  0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4,
  63  0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD,
  64  0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A,
  65  0xEAD54739, 0x9DD277AF, 0x04DB2615, 0x73DC1683,
  66  0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8,
  67  0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1,
  68  0xF00F9344, 0x8708A3D2, 0x1E01F268, 0x6906C2FE,
  69  0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7,
  70  0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC,
  71  0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43, 0x60B08ED5,
  72  0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252,
  73  0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B,
  74  0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6, 0x41047A60,
  75  0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79,
  76  0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236,
  77  0xCC0C7795, 0xBB0B4703, 0x220216B9, 0x5505262F,
  78  0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04,
  79  0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D,
  80  0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A,
  81  0x9C0906A9, 0xEB0E363F, 0x72076785, 0x05005713,
  82  0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38,
  83  0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21,
  84  0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8, 0x1FDA836E,
  85  0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777,
  86  0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C,
  87  0x8F659EFF, 0xF862AE69, 0x616BFFD3, 0x166CCF45,
  88  0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2,
  89  0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB,
  90  0xAED16A4A, 0xD9D65ADC, 0x40DF0B66, 0x37D83BF0,
  91  0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9,
  92  0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6,
  93  0xBAD03605, 0xCDD70693, 0x54DE5729, 0x23D967BF,
  94  0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94,
  95  0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D
  96 };
  97
  98 static uint32_t crc32_byte(uint8_t *p, uint32_t bytelength)
  99 {
 100     uint32_t crc = 0xffffffff;
 101     while (bytelength-- !=0) crc = poly8_lookup[((uint8_t) crc ^ *(p++))] ^ (crc >> 8);
 102     // return (~crc); also works
 103     return (crc ^ 0xffffffff);
 104 }
 105
 106 void validator_recv_frame(uint8_t link, uint8_t* data, uint16_t size) {
 107     if (size > 4) {
 108         uint32_t frame_crc;
 109         memcpy(&frame_crc, data + size -4, 4);
 110         uint32_t expected_crc = crc32_byte(data, size - 4);
 111         if (frame_crc == expected_crc) {
 112             route_incoming_frame(link, data, size-4);
 113         }
 114     }
 115 }
 116
 117 void validator_send_frame(uint8_t link, uint8_t* data, uint16_t size) {
 118     uint32_t crc = crc32_byte(data, size);
 119     memcpy(data + size, &crc, 4);
 120     byte_stuffer_send_frame(link, data, size + 4);
 121 }