components/api/src/mt_msg.c

   1 /******************************************************************************
   2  @file mt_msg.c
   3
   4  @brief TIMAC 2.0 API mt layer implimentation.
   5
   6  Group: WCS LPC
   7  $Target Devices: Linux: AM335x, Embedded Devices: CC1310, CC1350$
   8
   9  ******************************************************************************
  10  $License: BSD3 2016 $
  11
  12    Copyright (c) 2015, Texas Instruments Incorporated
  13    All rights reserved.
  14
  15    Redistribution and use in source and binary forms, with or without
  16    modification, are permitted provided that the following conditions
  17    are met:
  18
  19    *  Redistributions of source code must retain the above copyright
  20       notice, this list of conditions and the following disclaimer.
  21
  22    *  Redistributions in binary form must reproduce the above copyright
  23       notice, this list of conditions and the following disclaimer in the
  24       documentation and/or other materials provided with the distribution.
  25
  26    *  Neither the name of Texas Instruments Incorporated nor the names of
  27       its contributors may be used to endorse or promote products derived
  28       from this software without specific prior written permission.
  29
  30    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  31    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
  32    THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
  33    PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
  34    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  35    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  36    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
  37    OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
  38    WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
  39    OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  40    EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  41  ******************************************************************************
  42  $Release Name: TI-15.4Stack Linux x64 SDK$
  43  $Release Date: July 14, 2016 (2.00.00.30)$
  44  *****************************************************************************/
  45
  46 /******************************************************************************
  47  Includes
  48  *****************************************************************************/
  49
  50 #include "compiler.h"
  51 #include "mt_msg.h"
  52 #include "mt_msg_dbg.h"
  53 #include "log.h"
  54 #include "mutex.h"
  55 #include "threads.h"
  56 #include "stream.h"
  57 #include "stream_socket.h"
  58 #include "stream_uart.h"
  59 #include "timer.h"
  60 #include "ti_semaphore.h"
  61 #include "fatal.h"
  62
  63 #include <stdarg.h>
  64 #include <string.h>
  65 #include <stdlib.h>
  66
  67 /******************************************************************************
  68  Variables, globals & locals
  69  *****************************************************************************/
  70
  71 /* These are INI file log flag names, this array is used by "app_main"
  72   for the specific application (ie: npi_server, or appsrv/gateway)
  73   See the LOG.H file for details about log flags.
  74 */
  75 const struct ini_flag_name mt_msg_log_flags[] = {
  76     { .name = "mt-msg-traffic" , .value = LOG_DBG_MT_MSG_traffic },
  77     { .name = "mt-msg-raw"     , .value = LOG_DBG_MT_MSG_raw },
  78     { .name = "mt-msg-areq"    , .value = LOG_DBG_MT_MSG_areq },
  79     { .name = "mt-msg-fields"  , .value = LOG_DBG_MT_MSG_fields },
  80     { .name = "mt-msg-decode"  , .value = LOG_DBG_MT_MSG_decode },
  81     /* terminate */
  82     { .name = NULL }
  83 };
  84
  85 /*! used to track the order of messages. */
  86 static unsigned msg_sequence_counter;
  87
  88 /* Used to verify a msg pointer is a message pointer. */
  89 static const int msg_check_value = 'm';
  90
  91 /* Used to identify an incomming msg */
  92 static const char _incomming_msg[] = "incomming-msg";
  93
  94 struct mt_version_info MT_DEVICE_version_info;
  95
  96 /******************************************************************************
  97  Functions
  98  *****************************************************************************/
  99
 100 /*
 101    Public funnction in mt_msg.h - see mt_msg.h
 102  */
 103 void MT_MSG_init(void)
 104 {
 105     /* nothing todo, everything is done inside the */
 106     /* MT_MSG_IFaceInit() call */
 107 }
 108
 109 /*!
 110  * @brief calculate the checksum of the message
 111  * @param tf, ascii f specifies src interface
 112  * @param len - how long is the data payload
 113  * @returns xor checksum.
 114  */
 115 static int MT_MSG_calc_chksum(struct mt_msg *pMsg, int tf, int len)
 116 {
 117     bool b;
 118     int x;
 119     int chksum;
 120
 121     /* start at 1, just past the frame sync byte. */
 122     chksum = 0;
 123     /* ASSUME: there is no frame sync byte */
 124     x = 0;
 125
 126     if(tf == 'f') /* which interface do we use? */
 127     {
 128         b = pMsg->pSrcIface->frame_sync;
 129     }
 130     else
 131     {
 132         b = pMsg->pDestIface->frame_sync;
 133     }
 134     if(b)
 135     {
 136         /* start after the frame sync byte */
 137             x++;
 138     }
 139     for(/* above */ ; x < len ; x++)
 140     {
 141         chksum  ^= ((int)pMsg->iobuf[x]);
 142     }
 143     return (chksum & 0x0ff);
 144 }
 145
 146 /*
 147   Sets the message type in mt_msg::m_type
 148   see mt_msg.h
 149 */
 150 void MT_MSG_set_type(struct mt_msg *pMsg, struct mt_msg_interface *pMI)
 151 {
 152     int major;
 153     int minor;
 154     const int *iPtr;
 155     int x;
 156
 157     static const int lut_major[] = {
 158         MT_MSG_TYPE_poll,
 159         MT_MSG_TYPE_sreq,
 160         MT_MSG_TYPE_areq,
 161         MT_MSG_TYPE_srsp
 162     };
 163
 164     /* assume... */
 165     pMsg->m_type = MT_MSG_TYPE_unknown;
 166
 167     if((pMsg->cmd0 < 0) || (pMsg->cmd0 > 255))
 168     {
 169         return;
 170     }
 171     major = _bitsXYof(pMsg->cmd0, 7, 5);
 172     if((major >= 0) && (major <= 3))
 173     {
 174         pMsg->m_type = lut_major[major];
 175         return;
 176     }
 177
 178     /* to continue, we need the extended byte... */
 179
 180     /* need to know the associated interface */
 181     if(pMI == NULL)
 182     {
 183         return;
 184     }
 185
 186     /* Where does the payload start in the message? */
 187     x = (pMI->frame_sync ? 1 : 0) +
 188         (pMI->len_2bytes ? 2 : 1) +
 189         1 +
 190         1;
 191
 192     /* do we have the extension byte yet? */
 193     if(pMsg->iobuf_nvalid < x)
 194     {
 195         return; /* no */
 196     }
 197
 198     /* fetch the extension byte */
 199     minor = pMsg->iobuf[x];
 200
 201     /* extract the "minor" type */
 202     minor = _bitsXYof(minor, 7, 3);
 203
 204     /* assume it is not found */
 205     iPtr = NULL;
 206     switch (minor)
 207     {
 208     default:
 209         /* unknown */
 210         break;
 211     case 0: /* not used */
 212         break;
 213     case 1:
 214         /* stack specific  */
 215         {
 216             static const int stack_lut[] = {
 217                 MT_MSG_TYPE_unknown,
 218                 MT_MSG_TYPE_sreq_stack,
 219                 MT_MSG_TYPE_areq_stack,
 220                 MT_MSG_TYPE_srsp_stack
 221             };
 222             iPtr = stack_lut;
 223         }
 224         break;
 225     case 2:
 226         {
 227             /* fragmentation data */
 228             static const int frag_data_lut[] = {
 229                 MT_MSG_TYPE_unknown,
 230                 MT_MSG_TYPE_sreq_frag_data,
 231                 MT_MSG_TYPE_areq_frag_data,
 232                 MT_MSG_TYPE_srsp_frag_data
 233             };
 234             iPtr = frag_data_lut;
 235             break;
 236         }
 237     case 3:
 238         {
 239             /* Fragmentation ACK */
 240             static const int frag_ack_lut[] = {
 241                 MT_MSG_TYPE_unknown,
 242                 MT_MSG_TYPE_sreq_frag_ack,
 243                 MT_MSG_TYPE_areq_frag_ack,
 244                 MT_MSG_TYPE_srsp_frag_ack
 245             };
 246             iPtr = frag_ack_lut;
 247             break;
 248         }
 249     case 4:
 250         {
 251             /* Extended status */
 252             static const int ext_status_lut[] = {
 253                 MT_MSG_TYPE_unknown,
 254                 MT_MSG_TYPE_sreq_ext_status,
 255                 MT_MSG_TYPE_areq_ext_status,
 256                 MT_MSG_TYPE_srsp_ext_status
 257             };
 258             iPtr = ext_status_lut;
 259             break;
 260         }
 261     }
 262
 263     if(iPtr)
 264     {
 265         /* clear the extension bit */
 266         /* and get the major type.. */
 267         major = major & 0x03;
 268         if(iPtr[major] >= 0)
 269         {
 270             pMsg->m_type = iPtr[major];
 271         }
 272     }
 273 }
 274
 275 /*!
 276  * @brief Format a message for transmission.
 277  * @param pMsg
 278  *
 279  *  This inserts the correct byte sequence into
 280  *  the mt_msg::io_buf, including any frame sync
 281  *  and/or any checksums.
 282  */
 283 static void MT_MSG_format_msg(struct mt_msg *pMsg)
 284 {
 285     int n;
 286
 287     /* where does the payload start? */
 288     n = ((pMsg->pDestIface->frame_sync ? 1 : 0) +
 289          (pMsg->pDestIface->len_2bytes ? 2 : 1) +
 290          1 + /* cmd0 */
 291          1); /* cmd1 */
 292
 293     if(pMsg->iobuf_idx < 0)
 294     {
 295         /* no payload was ever written */
 296         pMsg->iobuf_idx = n;
 297     }
 298
 299     /* update the length if needed */
 300     if(pMsg->expected_len < 0)
 301     {
 302         pMsg->expected_len = pMsg->iobuf_idx - n;
 303     }
 304
 305     if( (pMsg->expected_len + n) != pMsg->iobuf_idx )
 306     {
 307         BUG_HERE("Expected len: %d, actual len: %d\n",
 308             (pMsg->expected_len + n), pMsg->iobuf_idx);
 309     }
 310
 311     /* reset io buffer so we can insert header */
 312     pMsg->iobuf_idx = 0;
 313
 314     /* frame sync */
 315     if(pMsg->pDestIface->frame_sync)
 316     {
 317         MT_MSG_wrU8(pMsg, 0xfe);
 318     }
 319
 320     /* Length */
 321     if(pMsg->pDestIface->len_2bytes)
 322     {
 323         MT_MSG_wrU16(pMsg, pMsg->expected_len);
 324     }
 325     else
 326     {
 327         MT_MSG_wrU8(pMsg, pMsg->expected_len);
 328     }
 329
 330     /* cmd0/cmd1 */
 331     MT_MSG_wrU8(pMsg, pMsg->cmd0);
 332     MT_MSG_wrU8(pMsg, pMsg->cmd1);
 333
 334     /* skip over the data/payload */
 335     MT_MSG_wrBuf(pMsg, NULL, pMsg->expected_len);
 336
 337     /* and capture the number of valid bytes */
 338     pMsg->iobuf_nvalid = pMsg->iobuf_idx;
 339
 340     /* if needed, the checksum */
 341     if(pMsg->pDestIface->include_chksum)
 342     {
 343         /* calculate the checksum */
 344         pMsg->chksum = MT_MSG_calc_chksum(pMsg, 't', pMsg->iobuf_nvalid);
 345         /* go back to the checksum location and write it */
 346         MT_MSG_wrU8(pMsg, pMsg->chksum);
 347     }
 348 }
 349
 350 /*
 351   Log a message
 352   see mt_msg.h
 353 */
 354 void MT_MSG_log(int64_t why, struct mt_msg *pMsg, const char *fmt, ...)
 355 {
 356     va_list ap;
 357     int x;
 358
 359     /* auto flag errors */
 360     if(why == LOG_ERROR)
 361     {
 362         pMsg->is_error = true;
 363     }
 364
 365     /* if we do not log ... leave */
 366     if(!LOG_test(why))
 367     {
 368         return;
 369     }
 370
 371     /* we do multiple things here so we lock/unlock */
 372     LOG_lock();
 373
 374     /* print the message */
 375     va_start(ap, fmt);
 376     LOG_vprintf(why, fmt, ap);
 377     va_end(ap);
 378
 379     /* if we have an error make this clear! */
 380     if(pMsg->is_error)
 381     {
 382         LOG_printf(why, "ERROR: ");
 383     }
 384
 385     /* do we have a prefix for this message? */
 386     if(pMsg->pLogPrefix)
 387     {
 388         LOG_printf(why, "%s ", pMsg->pLogPrefix);
 389     }
 390
 391     LOG_printf(why, "msg(%04x) nbytes=%d len=%d [",
 392                 (unsigned)(pMsg->sequence_id),
 393                 pMsg->iobuf_nvalid,
 394                 pMsg->expected_len);
 395     /* we print some bytes not all of the bytes */
 396     for(x = 0 ; (x < pMsg->iobuf_nvalid) && (x < 8) ; x++)
 397     {
 398         LOG_printf(why, " 0x%02x", pMsg->iobuf[x]);
 399     }
 400     LOG_printf(why, "]\n");
 401
 402     LOG_unLock();
 403 }
 404
 405 /*
 406   This function reformats a message (the payload) within a message
 407   it us used when forwarding a message from one interface to another
 408
 409   See mt_msg.h for more discussion.
 410 */
 411 void MT_MSG_reformat(struct mt_msg *pMsg)
 412 {
 413     int F_start;
 414     int T_start;
 415
 416     /* on the from side ... where does our payload begin? */
 417     F_start = (
 418         (pMsg->pSrcIface->frame_sync ? 1 : 0) +
 419         (pMsg->pSrcIface->len_2bytes ? 2 : 1) +
 420         1 + /* cmd0 */
 421         1); /* cmd1 */
 422
 423     /* on the to side ... where does our payload begin? */
 424     T_start = (
 425         (pMsg->pDestIface->frame_sync ? 1 : 0) +
 426         (pMsg->pDestIface->len_2bytes ? 2 : 1) +
 427         1 + /* cmd0 */
 428         1); /* cmd1 */
 429
 430     /* If we have payload... */
 431     if(pMsg->expected_len)
 432     {
 433         /* And it needs to move/shift */
 434         if(F_start != T_start)
 435         {
 436             /* then shift it */
 437             memmove(
 438                 (void *)(&(pMsg->iobuf[T_start])),
 439                 (void *)(&(pMsg->iobuf[F_start])),
 440                 pMsg->expected_len);
 441         }
 442     }
 443     pMsg->iobuf_nvalid = T_start + pMsg->expected_len;
 444
 445     /* adjust the write pointer */
 446     pMsg->iobuf_idx = T_start + pMsg->expected_len;
 447
 448     MT_MSG_log(LOG_DBG_MT_MSG_traffic, pMsg,
 449             "Reformatted msg from: %s to %s (fstart=%d, tstart=%d, len=%d)\n",
 450         pMsg->pSrcIface->dbg_name, pMsg->pDestIface->dbg_name,
 451         F_start, T_start, pMsg->iobuf_idx);
 452 }
 453
 454 /*!
 455  * @brief If the write index was not set, do that now.
 456  * @param pMsg - the message
 457  */
 458 static void init_wr_idx(struct mt_msg *pMsg)
 459 {
 460     /* if not set yet */
 461     if(pMsg->iobuf_idx >= 0)
 462     {
 463         return;
 464     }
 465
 466     /* set to payload */
 467     /* BECAUSE - we write payloads */
 468     pMsg->iobuf_idx = (
 469         (pMsg->pDestIface->frame_sync ? 1 : 0) + /* frame sync */
 470         /* if needed, 2nd len byte */
 471         (pMsg->pDestIface->len_2bytes ? 2 : 1) +
 472         1 + /* cmd 0 */
 473         1); /* cmd 1 */
 474 }
 475
 476 /*!
 477   Write an N-BIT value to the io stream
 478   see mt_msg.h
 479 */
 480 void MT_MSG_wrUX_DBG(struct mt_msg *pMsg, uint64_t value,
 481                       int nbits, const char *name)
 482 {
 483     int x;
 484
 485     if(pMsg->is_error)
 486     {
 487         return;
 488     }
 489     if(name)
 490     {
 491         LOG_printf(LOG_DBG_MT_MSG_fields, "%s: wr_u%d: %*s: %lld, 0x%llx\n",
 492                 pMsg->pLogPrefix,
 493                 nbits,
 494                 20,
 495                 name,
 496                 (long long)value,
 497                 (unsigned long long)value);
 498     }
 499
 500     /* set the write index */
 501     init_wr_idx(pMsg);
 502
 503     /* write data in little endian form */
 504     while(nbits)
 505     {
 506         x = pMsg->iobuf_idx;
 507         if(!_inrange(x, 0, pMsg->iobuf_idx_max))
 508         {
 509             pMsg->is_error = true;
 510             BUG_HERE("wr buf overflow\n");
 511             return;
 512         }
 513
 514         pMsg->iobuf[x] = (uint8_t)(value);
 515         x++;
 516         value = value >> 8;
 517         pMsg->iobuf_idx = x;
 518         pMsg->iobuf_nvalid = x;
 519         nbits -= 8;
 520
 521     }
 522 }
 523
 524 /*
 525   Write a U8 value to the mt_msg::io_buf
 526   see mt_msg.h
 527 */
 528 void MT_MSG_wrU8_DBG(struct mt_msg *pMsg, uint32_t value, const char *name)
 529 {
 530     MT_MSG_wrUX_DBG(pMsg, value, 8, name);
 531 }
 532
 533 /*
 534   Write a U16 value to the mt_msg::io_buf
 535   see mt_msg.h
 536 */
 537 void MT_MSG_wrU16_DBG(struct mt_msg *pMsg, uint32_t value, const char *name)
 538 {
 539     MT_MSG_wrUX_DBG(pMsg, value, 16, name);
 540 }
 541
 542 /*
 543   Write a U32 value to the mt_msg::io_buf
 544   see mt_msg.h
 545 */
 546 void MT_MSG_wrU32_DBG(struct mt_msg *pMsg, uint32_t value, const char *name)
 547 {
 548     MT_MSG_wrUX_DBG(pMsg, value, 32, name);
 549 }
 550
 551 /*
 552   Write a U64 value to the mt_msg::io_buf
 553   see mt_msg.h
 554 */
 555 void MT_MSG_wrU64_DBG(struct mt_msg *pMsg, uint64_t value, const char *name)
 556 {
 557     MT_MSG_wrUX_DBG(pMsg, value, 64, name);
 558 }
 559
 560 /*
 561   Write a byte buffer to the mt_msg::io_buf
 562   see mt_msg.h
 563 */
 564 void MT_MSG_wrBuf_DBG(struct mt_msg *pMsg, const void *pData,
 565                        size_t nbytes, const char *name)
 566 {
 567     if(pMsg->is_error)
 568     {
 569         return;
 570     }
 571
 572     /* init the index */
 573     init_wr_idx(pMsg);
 574
 575     /* are we done? */
 576     if(nbytes == 0)
 577     {
 578         return;
 579     }
 580
 581     /* do we go to far? */
 582     if((pMsg->iobuf_idx + ((int)nbytes)) > pMsg->iobuf_idx_max)
 583     {
 584         pMsg->is_error = true;
 585         BUG_HERE("msg wr overflow\n");
 586         return;
 587     }
 588
 589     if(name)
 590     {
 591         LOG_printf(LOG_DBG_MT_MSG_fields, "%s: wrBuf: %*s, len: %d\n",
 592                     pMsg->pLogPrefix, 20, name, (int)nbytes);
 593     }
 594     /* is this a dummy or real write? */
 595     if(pData)
 596     {
 597         /* we might be doing a dummy write */
 598         memcpy((void *)(&(pMsg->iobuf[pMsg->iobuf_idx])), pData, nbytes);
 599     }
 600     pMsg->iobuf_idx += (int)nbytes;
 601     pMsg->iobuf_nvalid += (int)nbytes;
 602 }
 603
 604 /*
 605   Peek (ahead) into the message at a specific byte offset.
 606   see mt_msg.h
 607 */
 608 int MT_MSG_Peek_u8_DBG(struct mt_msg *pMsg, int ofset, const char *name)
 609 {
 610     int x;
 611
 612     /* if not set yet */
 613     if(pMsg->iobuf_idx < 0)
 614     {
 615         /* set to start of message */
 616         /* Because we parse messages */
 617         pMsg->iobuf_idx = 0;
 618     }
 619
 620     x = pMsg->iobuf_idx + ofset;
 621     if(!_inrange(x, 0, pMsg->iobuf_nvalid))
 622     {
 623         return (EOF);
 624     }
 625     x = pMsg->iobuf[x] & 0x0ff;
 626
 627     LOG_printf(LOG_DBG_MT_MSG_fields, "%s: peek_u8: %s: %d, 0x%02x\n",
 628                 pMsg->pLogPrefix,
 629                 name,
 630                 (int)x,
 631                 (int)x);
 632
 633     return (x);
 634 }
 635
 636 /*
 637   Read an N-bit value from the mt_msg::io_buf
 638   see mt_msg.h
 639 */
 640 uint64_t MT_MSG_rdUX_DBG(struct mt_msg *pMsg, int nbits, const char *name)
 641 {
 642     int x;
 643     int y;
 644     uint64_t v;
 645     uint64_t v2;
 646
 647     if(pMsg->is_error)
 648     {
 649         return (0);
 650     }
 651
 652     /* if not set yet */
 653     if(pMsg->iobuf_idx < 0)
 654     {
 655         /* set to start of message */
 656         /* because we parse messages */
 657         pMsg->iobuf_idx = 0;
 658     }
 659
 660     v = 0;
 661
 662     /* Data is transmitted LSB first */
 663     for(x = 0 ; x < nbits ; x += 8)
 664     {
 665         y = pMsg->iobuf_idx;
 666         if(!_inrange(y, 0, pMsg->iobuf_nvalid))
 667         {
 668             v = (uint64_t)-1;
 669             BUG_HERE("msg rd underflow\n");
 670             break;
 671         }
 672         v2 = pMsg->iobuf[y];
 673         y++;
 674         pMsg->iobuf_idx = y;
 675
 676         v2 = v2 << x;
 677         v = v | v2;
 678     }
 679
 680     if(name)
 681     {
 682         LOG_printf(LOG_DBG_MT_MSG_fields, "%s: rd_u%d: %*s: %5lld, 0x%0*llx\n",
 683                     pMsg->pLogPrefix,
 684                     nbits,
 685                     12,
 686                     name,
 687                     (long long)v,
 688                     (nbits / 4),
 689                     (unsigned long long)v);
 690     }
 691     return (v);
 692 }
 693
 694 /*
 695   Read a U8 value from the mt_msg::io_buf
 696   see mt_msg.h
 697  */
 698 uint8_t MT_MSG_rdU8_DBG(struct mt_msg *pMsg, const char *name)
 699 {
 700     return ((uint8_t)MT_MSG_rdUX_DBG(pMsg, 8, name));
 701 }
 702
 703 /*
 704  Read a U16 value from the mt_msg::io_buf
 705  see mt_msg.h
 706 */
 707 uint16_t MT_MSG_rdU16_DBG(struct mt_msg *pMsg, const char *name)
 708 {
 709     return ((uint16_t)MT_MSG_rdUX_DBG(pMsg, 16, name));
 710 }
 711
 712 /*
 713  Read a U32 value from the mt_msg::io_buf
 714  see mt_msg.h
 715 */
 716 uint32_t MT_MSG_rdU32_DBG(struct mt_msg *pMsg, const char *name)
 717 {
 718     return ((uint32_t)MT_MSG_rdUX_DBG(pMsg, 32, name));
 719 }
 720
 721 /*
 722  Read a U64 value from the mt_msg::io_buf
 723  see mt_msg.h
 724 */
 725 uint64_t MT_MSG_rdU64_DBG(struct mt_msg *pMsg, const char *name)
 726 {
 727     return (MT_MSG_rdUX_DBG(pMsg, 64, name));
 728 }
 729
 730 /*
 731  Parsing is complete, is everything ok?
 732  See mt_msg.h
 733 */
 734 void MT_MSG_parseComplete(struct mt_msg *pMsg)
 735 {
 736     int n;
 737
 738     /* where should the iobuf_idx be? */
 739     n = (pMsg->pSrcIface->frame_sync ? 1 : 0) +  /* sync byte */
 740         (pMsg->pSrcIface->len_2bytes ? 2 : 1) +
 741         1 + /* cmd0 */
 742         1 + /* cmd1 */
 743         /* finally the payload */
 744         pMsg->expected_len;
 745
 746     /* hmm is it wrong? */
 747     if(pMsg->iobuf_idx != n)
 748     {
 749         /* then we have an error */
 750         pMsg->is_error = true;
 751         MT_MSG_log(LOG_ERROR, pMsg, "%s: incomplete parse\n",
 752                     pMsg->pSrcIface->dbg_name);
 753         BUG_HERE("incomplete parse");
 754     }
 755 }
 756
 757 /*
 758   Read a chunk of bytes from the mt_msg::io_buf
 759   see mt_msg.h
 760 */
 761 void MT_MSG_rdBuf_DBG(struct mt_msg *pMsg,
 762                       void *pData,
 763                       size_t nbytes,
 764                       const char *name)
 765 {
 766     /* we don't read any more if we hit an error */
 767     if(pMsg->is_error)
 768     {
 769         return;
 770     }
 771
 772     /* if not set yet */
 773     if(pMsg->iobuf_idx < 0)
 774     {
 775         /* set to start of message */
 776         /* because we parse complete messages */
 777         pMsg->iobuf_idx = 0;
 778     }
 779
 780     if(nbytes == 0)
 781     {
 782         return;
 783     }
 784
 785     /* are we reading past the end? */
 786     if((pMsg->iobuf_idx + ((int)nbytes)) > pMsg->iobuf_idx_max)
 787     {
 788         pMsg->is_error = true;
 789         BUG_HERE("msg rd underflow\n");
 790         return;
 791     }
 792
 793     if(name)
 794     {
 795         LOG_printf(LOG_DBG_MT_MSG_fields, "%s: rdBuf: %*s, len: %d\n",
 796                     pMsg->pLogPrefix, 20, name, (int)nbytes);
 797     }
 798
 799     /* pData might be NULL if we are doing a "dummy read" */
 800     /* meaning: Sometimes we need to skip over payload bytes */
 801     if(pData)
 802     {
 803         memcpy(pData, (void *)(&(pMsg->iobuf[pMsg->iobuf_idx])), nbytes);
 804     }
 805     pMsg->iobuf_idx += (int)nbytes;
 806 }
 807
 808 /*
 809   Release this message back into the heap
 810   see mt_msg.h
 811 */
 812 void MT_MSG_free(struct mt_msg *pMsg)
 813 {
 814     if(pMsg == NULL)
 815     {
 816         return;
 817     }
 818
 819     if(pMsg->check_ptr != &(msg_check_value))
 820     {
 821         BUG_HERE("not a message ptr: %p\n", (void *)(pMsg));
 822         return;
 823     }
 824
 825     /* release everything in this list */
 826     if(pMsg->pListNext)
 827     {
 828         MT_MSG_free(pMsg->pListNext);
 829         pMsg->pListNext = NULL;
 830     }
 831
 832     /* and the srsp for this message */
 833     if(pMsg->pSrsp)
 834     {
 835         MT_MSG_free(pMsg->pSrsp);
 836         pMsg->pSrsp = NULL;
 837     }
 838
 839     /* make it unusable. */
 840     memset((void *)(pMsg), 0, sizeof(*pMsg));
 841     free((void *)pMsg);
 842 }
 843
 844 /*!
 845  * @brief Local function to reset a message so we can reuse it.
 846  * @param pMsg - the message
 847  * @param len  - expected length
 848  * @param cmd0 - value for command 0
 849  * @param cmd1 - value for command 1
 850  *
 851  * Also see MT_MSG_alloc()
 852  */
 853 static void MT_MSG_resetMsg(struct mt_msg *pMsg, int len, int cmd0, int cmd1)
 854 {
 855     pMsg->is_error = false;
 856     pMsg->cmd0 = cmd0;
 857     pMsg->cmd1 = cmd1;
 858
 859     /* set length unknown */
 860     /* will be set upon first rd/wr of message */
 861     pMsg->iobuf_idx = -1;
 862     pMsg->iobuf_nvalid = 0;
 863
 864     MT_MSG_set_type(pMsg, NULL);
 865
 866     /* Set the length */
 867     pMsg->expected_len = len;
 868     if(len < 0)
 869     {
 870         /*----------------------------------------
 871           NOTE: LEN might be negative.
 872                 Why? Because the size is not known yet.
 873          ---------------------------------------- */
 874     }
 875     else
 876     {
 877         /* sanity check Total size
 878             1 = sync byte
 879             2 = len byte (worse case)
 880             1 = cmd0
 881             1 = cmd1
 882            *len* = provided
 883             1 = checksum
 884           ======
 885             5 + len
 886         */
 887         if((len + 5) > sizeof(pMsg->iobuf))
 888         {
 889             BUG_HERE("msg too big\n");
 890         }
 891     }
 892
 893 }
 894
 895 /*
 896   Allocate (from heap) a message, initialize as required
 897   see mt_msg.h
 898 */
 899 struct mt_msg *MT_MSG_alloc(int len, int cmd0, int cmd1)
 900 {
 901     struct mt_msg *pMsg;
 902
 903     /* get memory */
 904     pMsg = calloc(1, sizeof(*pMsg));
 905     if(pMsg == NULL)
 906     {
 907         BUG_HERE("no memory\n");
 908     }
 909
 910     /* initialize it */
 911     if(pMsg)
 912     {
 913         pMsg->sequence_id = msg_sequence_counter++;
 914         pMsg->check_ptr = &(msg_check_value);
 915         /* current implimentation is a u8 array local to the struct */
 916         pMsg->iobuf_idx_max = sizeof(pMsg->iobuf);
 917
 918         MT_MSG_resetMsg(pMsg, len, cmd0, cmd1);
 919     }
 920     return (pMsg);
 921 }
 922
 923 /*
 924   Clone (copy) a message
 925
 926   Public function defined in mt_msg
 927 */
 928 struct mt_msg *MT_MSG_clone(struct mt_msg *pOrig)
 929 {
 930     struct mt_msg_interface *pMI;;
 931     struct mt_msg *pClone;
 932     int save_id;
 933
 934     pClone = MT_MSG_alloc(pOrig->expected_len, pOrig->cmd0, pOrig->cmd1);
 935     if(pClone == NULL)
 936     {
 937         MT_MSG_log(LOG_ERROR, pOrig, "clone failed no memory\n");
 938         return (NULL);
 939     }
 940
 941     /* save this */
 942     save_id = pClone->sequence_id;
 943
 944     /* make sure it has an interface of some type */
 945     pMI = pOrig->pSrcIface;
 946     if(pMI == NULL)
 947     {
 948         pMI = pOrig->pDestIface;
 949     }
 950     if(pMI == NULL)
 951     {
 952         FATAL_printf("this message has no interface\n");
 953     }
 954
 955     LOG_printf(LOG_DBG_MT_MSG_traffic,
 956         "MT_MSG: clone(%s, id: %d) to: id: %d\n",
 957         pMI->dbg_name,
 958         pOrig->sequence_id,
 959         save_id);
 960
 961     *pClone = *pOrig;
 962     pClone->sequence_id = save_id;
 963
 964     if(pClone->pSrsp)
 965     {
 966         pClone->pSrsp = MT_MSG_clone(pClone->pSrsp);
 967     }
 968
 969     /* The clone is not in a list it is new. */
 970     pClone->pListNext = NULL;
 971
 972     pClone->was_formatted = false;
 973     /* Done */
 974     return (pClone);
 975 }
 976
 977 /*
 978  * @brief Transmit a message
 979  * @param pMsg - the message t transmit
 980  * @returns number of messages transmitteed (1=success)
 981  */
 982 static int MT_MSG_tx_raw(struct mt_msg *pMsg)
 983 {
 984     int r;
 985
 986     /* insert frame sync, cmd0/1 and checksum */
 987     MT_MSG_format_msg(pMsg);
 988
 989     LOG_lock();
 990     MT_MSG_dbg_decode(pMsg, pMsg->pDestIface, ALL_MT_MSG_DBG);
 991
 992     MT_MSG_log(LOG_DBG_MT_MSG_traffic, pMsg, "%s: TX Msg (start) [%s]\n",
 993                pMsg->pDestIface->dbg_name,
 994                pMsg->pLogPrefix);
 995
 996     if(LOG_test(LOG_DBG_MT_MSG_raw))
 997     {
 998         LOG_printf(LOG_DBG_MT_MSG_raw, "%s: TX %d bytes\n",
 999                    pMsg->pDestIface->dbg_name,
1000                    pMsg->iobuf_nvalid);
1001         LOG_hexdump(LOG_DBG_MT_MSG_raw, 0, pMsg->iobuf, pMsg->iobuf_nvalid);
1002     }
1003     LOG_unLock();
1004
1005     /* send the bytes */
1006     r = STREAM_wrBytes(pMsg->pDestIface->hndl,
1007                         (void *)(pMsg->iobuf),
1008                         pMsg->iobuf_nvalid, -1);
1009
1010     LOG_printf(LOG_DBG_MT_MSG_traffic,
1011                 "%s: TX Msg (Complete) r=%d [%s]\n",
1012                 pMsg->pDestIface->dbg_name, r,
1013                 pMsg->pLogPrefix);
1014     /* great success? */
1015     if(r == pMsg->iobuf_nvalid)
1016     {
1017         /* we transmitted 1 message */
1018         return (1);
1019     }
1020
1021     MT_MSG_log(LOG_ERROR, pMsg, "%s: cannot transmit r=%d\n",
1022         pMsg->pDestIface->dbg_name,
1023         r);
1024     /* we transmitted 0 messages */
1025     return (0);
1026 }
1027
1028 /*
1029  * @brief common code to send an extended status packet.
1030  * @param pMI - the message interface
1031  * @param pFI - the fragmentation info to use (tx or rx)
1032  * @param blocktype - either 3(frag ack) or 4(extended status)
1033  * @param statuscode - value to send.
1034  */
1035 static void common_send_status(struct mt_msg_interface *pMI,
1036                                 struct mt_msg_iface_frag_info *pFI,
1037                                 int blocktype, int statuscode)
1038 {
1039     struct mt_msg *pStatus;
1040
1041     /* get a message to send */
1042     pStatus = MT_MSG_alloc(3, pFI->pMsg->cmd0 | _bit7, pFI->pMsg->cmd1);
1043     if(pStatus == NULL)
1044     {
1045         pFI->is_error = true;
1046         return;
1047     }
1048
1049     /* fill in the message */
1050     pStatus->pLogPrefix = "frag-status";
1051
1052     MT_MSG_setDestIface(pStatus, pMI);
1053
1054     MT_MSG_wrU8(pStatus, (blocktype << 3) | pMI->stack_id);
1055     MT_MSG_wrU8(pStatus, pFI->block_cur);
1056     MT_MSG_wrU8(pStatus, statuscode);
1057     /* and send it */
1058     MT_MSG_tx_raw(pStatus);
1059
1060     /* and release it */
1061     MT_MSG_free(pStatus);
1062     pStatus = NULL;
1063 }
1064
1065 /*!
1066  * @brief Send an extended status packet.
1067  * @param pMI - the interface
1068  * @param pFI - the fragment info (tx or rx)
1069  * @param statuscode - what status to send
1070  */
1071 static void send_extended_status(struct mt_msg_interface *pMI,
1072                                  struct mt_msg_iface_frag_info *pFI,
1073                                  int statuscode)
1074 {
1075     // 4 is the extended status message
1076     common_send_status(pMI, pFI, 4, statuscode);
1077 }
1078
1079 /*!
1080  * @brief send a normal frag_ack packet
1081  * @param pMI - the interface
1082  * @param pFI - the fragment info (tx or rx)
1083  * @param statuscode - what status to send
1084  */
1085 static void send_frag_ack_packet(struct mt_msg_interface *pMI,
1086                                  struct mt_msg_iface_frag_info *pFI,
1087                                  int statuscode)
1088 {
1089     // 3 is the "frag-ack" message
1090     common_send_status(pMI, pFI, 3, statuscode);
1091 }
1092
1093 /*!
1094  * @brief Abort with an out of order message
1095  * @param pMI - the interface
1096  * @param pFI - the fragment info (tx or rx)
1097  */
1098 static void send_frag_abort_outoforder(struct mt_msg_interface *pMI,
1099                                        struct mt_msg_iface_frag_info *pFI)
1100 {
1101     send_frag_ack_packet(pMI, pFI, MT_MSG_FRAG_STATUS_block_out_of_order);
1102 }
1103
1104 /*!
1105  * @brief Wait for the fragment ack to occur (or a timeout, or an error)
1106  * @param pMI - the interface in use
1107  * @return 0 on succes
1108  */
1109 static int wait_for_frag_ack(struct mt_msg_interface *pMI)
1110 {
1111     int r;
1112     struct mt_msg *pAck;
1113     int ack_block;
1114     int ack_status;
1115
1116     /* Fragments are like sreq/srsp... */
1117     /* so we use the srsp timeout here */
1118     LOG_printf(LOG_DBG_MT_MSG_traffic, "Waiting for frag-ack\n");
1119
1120     SEMAPHORE_waitWithTimeout(pMI->tx_frag.tx_ack_semaphore,
1121                               pMI->srsp_timeout_mSecs);
1122
1123     MUTEX_lock(pMI->list_lock,-1);
1124     pAck = pMI->tx_frag.pTxFragAck;
1125     if(pAck)
1126     {
1127         pMI->tx_frag.pTxFragAck = pAck->pListNext;
1128     }
1129     MUTEX_unLock(pMI->list_lock);
1130
1131     if(pAck == NULL)
1132     {
1133         LOG_printf(LOG_DBG_MT_MSG_traffic, "timeout: frag-ack\n");
1134         /* no response */
1135         /* we send the current block again. */
1136         /* do not advance */
1137         r = 0;
1138         goto done;
1139     }
1140
1141     /* ignore the extended version byte */
1142     MT_MSG_rdU8(pAck);
1143     /* read blocknumber */
1144     ack_block = MT_MSG_rdU8(pAck);
1145     ack_status = MT_MSG_rdU8(pAck);
1146     MT_MSG_parseComplete(pAck);
1147     if(pAck->is_error)
1148     {
1149         /* we are toast.. */
1150         /* out of sequence abort */
1151         send_frag_abort_outoforder(pMI,&(pMI->tx_frag));
1152         /* we are dead */
1153         pMI->tx_frag.is_error = true;
1154         /* do not advance */
1155         r = 0;
1156         goto done;
1157     }
1158     /* resend last block? */
1159     if(ack_status == MT_MSG_FRAG_STATUS_resend_last)
1160     {
1161         r = 0; /* do not advance */
1162         goto done;
1163     }
1164     /* success? */
1165     if((ack_status == MT_MSG_FRAG_STATUS_success) ||
1166          (ack_status == MT_MSG_FRAG_STATUS_frag_complete))
1167     {
1168         /* send next block */
1169         if(ack_block == pMI->tx_frag.block_cur)
1170         {
1171             /* yea, we advance */
1172             r = 1;
1173             goto done;
1174         }
1175     }
1176     pMI->tx_frag.is_error = true;
1177     MT_MSG_log(LOG_ERROR, pAck, "block:%d, bad ack status: %d\n",
1178                ack_block, ack_status);
1179     /* we don't understand the status.. */
1180     send_frag_abort_outoforder(pMI, &(pMI->tx_frag));
1181     /* do not advance */
1182     r = 0;
1183 done:
1184     if(pAck)
1185     {
1186         MT_MSG_free(pAck);
1187         pAck = NULL;
1188     }
1189     return (r);
1190 }
1191
1192 /*!
1193  * @brief Send a fragment chunk..
1194  * @param pMI - the interface in use.
1195  * Also see wait_for_frag_ack()
1196  */
1197 static void frag_tx_one_block(struct mt_msg_interface *pMI)
1198 {
1199     int rd_offset;
1200     int n;
1201
1202     /* housekeeping.. */
1203     pMI->tx_frag.pTxFragData->pLogPrefix = "frag-data";
1204     LOG_printf(LOG_DBG_MT_MSG_traffic, "TX: %s:(frag) block: %d of %d\n",
1205         pMI->dbg_name, pMI->tx_frag.block_cur+1, pMI->tx_frag.block_count);
1206
1207     /* reset the data packet. */
1208     MT_MSG_resetMsg(pMI->tx_frag.pTxFragData, -1,
1209                     pMI->tx_frag.pMsg->cmd0 | _bit7, pMI->tx_frag.pMsg->cmd1);
1210
1211     /* set interfaces */
1212
1213     /* it has no source */
1214     MT_MSG_setSrcIface(pMI->tx_frag.pTxFragData, NULL);
1215     /* it has an interface */
1216     MT_MSG_setDestIface(pMI->tx_frag.pTxFragData, pMI);
1217
1218     /* insert the extended header */
1219     MT_MSG_wrU8(pMI->tx_frag.pTxFragData, (2 << 3) | pMI->stack_id);
1220
1221     MT_MSG_wrU8(pMI->tx_frag.pTxFragData, pMI->tx_frag.block_cur);
1222     MT_MSG_wrU16(pMI->tx_frag.pTxFragData, pMI->tx_frag.total_size);
1223
1224     /* where do we get our data? */
1225     rd_offset = (pMI->tx_frag.block_cur * pMI->tx_frag.this_frag_size);
1226
1227     /* do not send more then the fragment size */
1228     n = pMI->tx_frag.pMsg->expected_len - rd_offset;
1229     if(n > pMI->tx_frag.this_frag_size)
1230     {
1231         n = pMI->tx_frag.this_frag_size;
1232     }
1233
1234     /* Adjust rd offset to just after the packet header */
1235     rd_offset +=
1236         (pMI->frame_sync ? 1 : 0) +
1237         (pMI->len_2bytes ? 2 : 1) +
1238         1 + /* cmd0 */
1239         1; /* cmd1 */
1240
1241     /* now insert data */
1242     MT_MSG_wrBuf(pMI->tx_frag.pTxFragData,
1243                   pMI->tx_frag.pMsg->iobuf + rd_offset, n);
1244     MT_MSG_set_type(pMI->tx_frag.pTxFragData, pMI);
1245     MT_MSG_tx_raw(pMI->tx_frag.pTxFragData);
1246 }
1247
1248 /*!
1249  * @brief  transmit a fragment block and recevie the ack
1250  * @param pMI - interface to handle the fragment block
1251  * @returns number of frag blocks sent (success=1)
1252  *  Return 1 to advance to the block
1253  *  Return 0 to retransmit the current block
1254  *  set "is_error" if we should abandon
1255  */
1256 static int frag_txrx_one_block(struct mt_msg_interface *pMI)
1257 {
1258     int trynum;
1259     int r;
1260
1261     r = 0;
1262     for(trynum = 0; trynum < pMI->retry_max; trynum++)
1263     {
1264         LOG_printf(LOG_DBG_MT_MSG_traffic,
1265             "TX: Block: %d of %d, Try: %d of %d\n",
1266             pMI->tx_frag.block_cur+1, pMI->tx_frag.block_count,
1267             trynum, pMI->retry_max);
1268
1269         frag_tx_one_block(pMI);
1270         if(pMI->tx_frag.is_error)
1271         {
1272             break;
1273         }
1274
1275         r = wait_for_frag_ack(pMI);
1276         /* if success or error, we are done */
1277         if((r > 0) || (pMI->tx_frag.is_error))
1278         {
1279             break;
1280         }
1281     }
1282     return (r);
1283 }
1284
1285 /*!
1286  * @brief Chop up, loop over, and transmit a message via fragmentation
1287  * @param pMsg - msg to transmit
1288  * @return 1 - on success 1 [big] message was transmitted.
1289  */
1290 static int MT_MSG_tx_fragment(struct mt_msg *pMsg)
1291 {
1292     int r;
1293     struct mt_msg_interface *pMI;
1294
1295     pMI = pMsg->pDestIface;
1296
1297     pMI->tx_frag.pMsg = pMsg;
1298     pMI->tx_frag.is_error = false;
1299
1300     pMI->tx_frag.total_size = pMsg->expected_len;
1301     pMI->tx_frag.this_frag_size = pMsg->pDestIface->tx_frag_size;
1302     pMI->tx_frag.block_cur = 0;
1303     pMI->tx_frag.block_count =
1304         (pMsg->expected_len + pMI->tx_frag.this_frag_size - 1) /
1305         pMI->tx_frag.this_frag_size;
1306
1307     pMI->tx_frag.pTxFragData = MT_MSG_alloc(-1,
1308                                              pMsg->cmd0 | _bit7,
1309                                              pMsg->cmd1);
1310     if(pMI->tx_frag.pTxFragData == NULL)
1311     {
1312         MT_MSG_log(LOG_ERROR, pMsg, "no memory to fragment\n");
1313         pMI->tx_frag.is_error = true;
1314     }
1315     else
1316     {
1317         /* for each block.. */
1318         r = 0;
1319         for(pMI->tx_frag.block_cur = 0;
1320             pMI->tx_frag.block_cur < pMI->tx_frag.block_count;
1321                 pMI->tx_frag.block_cur += r)
1322         {
1323
1324             if(pMI->tx_frag.is_error)
1325             {
1326                 break;
1327             }
1328
1329             /* transfer the block */
1330             r = frag_txrx_one_block(pMI);
1331             /* this returns: */
1332             /* 0 - resend current block */
1333             /* or +1 to send the next block */
1334         }
1335     }
1336
1337     /* what is our result? */
1338     if(pMI->tx_frag.is_error)
1339     {
1340         send_extended_status(pMI, &(pMI->tx_frag),
1341                               MT_MSG_EXT_STATUS_frag_aborted);
1342         /* we transmitted 0 packets */
1343         r = 0;
1344     }
1345     else
1346     {
1347         /* we send the last block *NUMBER* not the *COUNT*
1348            Due to the for() loop above
1349            "block_cur == block_count"
1350            We must send the last block number
1351            not the *block*count* in the extended status.
1352         */
1353         pMI->tx_frag.block_cur--;
1354         send_extended_status(pMI, &(pMI->tx_frag),
1355                               MT_MSG_EXT_STATUS_frag_complete);
1356         /* we transmitted 1 packet */
1357         r = 1;
1358     }
1359
1360     /* cleanup */
1361     pMI->tx_frag.pMsg = NULL;
1362     pMI->tx_frag.pTxFragAck = NULL;
1363
1364     if(pMI->tx_frag.pTxFragData)
1365     {
1366         MT_MSG_free(pMI->tx_frag.pTxFragData);
1367         pMI->tx_frag.pTxFragData = NULL;
1368     }
1369
1370     return (r);
1371 }
1372
1373 /*!
1374  * @brief Transmit a message [this is not rx, see MT_MSG_txrx()]
1375  * @param pMsg - the message to transmit
1376  * @return 1 upon success [1 msg transmitted]
1377  */
1378 static int MT_MSG_tx(struct mt_msg *pMsg)
1379 {
1380     int r;
1381     /* Set the message type */
1382     MT_MSG_set_type(pMsg, pMsg->pDestIface);
1383
1384     /* if there was an error, we don't tx */
1385     if(pMsg->is_error || (pMsg->m_type == MT_MSG_TYPE_unknown))
1386     {
1387         /* we transmitted ZERO packets. */
1388         r = 0;
1389         goto done;
1390     }
1391
1392     /* Do we fragment? */
1393     if(pMsg->pDestIface->len_2bytes)
1394     {
1395         /* we don't need to check for fragmentation */
1396     }
1397     else
1398     {
1399         if((pMsg->iobuf_nvalid > 256) ||
1400             (pMsg->iobuf_nvalid >= pMsg->pDestIface->tx_frag_size))
1401         {
1402             /* yes, we fragment */
1403             r = MT_MSG_tx_fragment(pMsg);
1404             goto done;
1405         }
1406     }
1407
1408     r = MT_MSG_tx_raw(pMsg);
1409 done:
1410     return (r);
1411 }
1412
1413 /*
1414   release resources for this message interface
1415   see mt_msg.h
1416 */
1417 void MT_MSG_interfaceDestroy(struct mt_msg_interface *pMI)
1418 {
1419     if(pMI == NULL)
1420     {
1421         return;
1422     }
1423
1424     LOG_printf(LOG_DBG_MT_MSG_traffic,
1425                "%s: Destroy interface\n", pMI->dbg_name);
1426     pMI->is_dead = true;
1427
1428     /* kill off any messages we have in process */
1429     if(pMI->pCurRxMsg)
1430     {
1431         MT_MSG_free(pMI->pCurRxMsg);
1432         pMI->pCurRxMsg = NULL;
1433     }
1434
1435     /* close our connection */
1436     if(pMI->hndl)
1437     {
1438         STREAM_close(pMI->hndl);
1439         /* is this a socket connection? */
1440         if(pMI->s_cfg)
1441         {
1442             if(pMI->s_cfg->ascp == 'c')
1443             {
1444                 SOCKET_CLIENT_destroy(pMI->hndl);
1445             }
1446             else
1447             {
1448                 SOCKET_SERVER_destroy(pMI->hndl);
1449             }
1450         }
1451         pMI->hndl = 0;
1452     }
1453
1454     /* and our rx thread */
1455     if(pMI->rx_thread)
1456     {
1457         THREAD_destroy(pMI->rx_thread);
1458         pMI->rx_thread = 0;
1459     }
1460
1461     /* any pending message are tossed */
1462     MT_MSG_LIST_destroy(&(pMI->rx_list));
1463
1464     /* our lock */
1465     if(pMI->list_lock)
1466     {
1467         MUTEX_destroy(pMI->list_lock);
1468         pMI->list_lock = 0;
1469     }
1470
1471     /* our SREQ/SRSP semaphore */
1472     if(pMI->srsp_semaphore)
1473     {
1474         SEMAPHORE_destroy(pMI->srsp_semaphore);
1475         pMI->srsp_semaphore = 0;
1476     }
1477
1478     /* our fragmentation semaphore */
1479     if(pMI->tx_frag.tx_ack_semaphore)
1480     {
1481         SEMAPHORE_destroy(pMI->tx_frag.tx_ack_semaphore);
1482         pMI->tx_frag.tx_ack_semaphore = 0;
1483     }
1484
1485     if(pMI->tx_lock)
1486     {
1487         MUTEX_destroy(pMI->tx_lock);
1488         pMI->tx_lock = 0;
1489     }
1490
1491     /* Any pending sreq is dead */
1492     MT_MSG_free(pMI->pCurSreq);
1493     pMI->pCurSreq = NULL;
1494
1495     /* we do *NOT* zap (zero) the interface. */
1496     /* The caller may need to release destroy the handle. */
1497 }
1498
1499 /*!
1500  * @brief Read N bytes from the interface and insert into the message
1501  * @param pMsg - the message to insert into
1502  * @param n - how many bytes we need
1503  * @param timeout_mSecs - how long to wait
1504  * @returns nbytes valid in the rx buffer (total)
1505  */
1506 static int mt_msg_rx_bytes(struct mt_msg_interface *pMI,
1507                             int n, int timeout_mSecs)
1508 {
1509     struct mt_msg *pMsg;
1510     int r;
1511     int nneed;
1512
1513     pMsg = pMI->pCurRxMsg;
1514
1515     /* do we already have enough? */
1516     if(pMsg->iobuf_nvalid >= n)
1517     {
1518         return (n);
1519     }
1520     /* how many *MORE* do we need? */
1521     nneed = n - pMsg->iobuf_nvalid;
1522
1523     /* go read */
1524     r = STREAM_rdBytes(pMI->hndl,
1525                        pMsg->iobuf + pMsg->iobuf_nvalid,
1526                        nneed,
1527                        timeout_mSecs);
1528     if(r > 0)
1529     {
1530         /* great success? */
1531         pMsg->iobuf_nvalid += r;
1532     }
1533     if(r <= 0)
1534     {
1535         if(STREAM_isSocket(pMI->hndl))
1536         {
1537             /* did we get a tcpip disconnect? */
1538             if(!STREAM_SOCKET_isConnected(pMI->hndl))
1539             {
1540                 LOG_printf(LOG_DBG_MT_MSG_traffic,
1541                            "%s: Socket is dead\n",
1542                            pMI->dbg_name);
1543                 pMI->is_dead = true;
1544             }
1545         }
1546         else
1547         {
1548             if(r < 0)
1549             {
1550                 /* USB uarts die if they are disconnected */
1551                 pMI->is_dead = true;
1552             }
1553         }
1554     }
1555     /* log ... */
1556     if(pMsg->iobuf_nvalid)
1557     {
1558         if(LOG_test(LOG_DBG_MT_MSG_raw))
1559         {
1560             LOG_printf(LOG_DBG_MT_MSG_raw,
1561                        "%s: nbytes-avail: %d\n",
1562                        pMI->dbg_name,
1563                        pMsg->iobuf_nvalid);
1564             LOG_hexdump(LOG_DBG_MT_MSG_raw,
1565                         0,
1566                         pMI->pCurRxMsg->iobuf,
1567                         pMI->pCurRxMsg->iobuf_nvalid);
1568         }
1569     }
1570
1571     return (pMsg->iobuf_nvalid);
1572 }
1573
1574 /*!
1575  * @brief - read a message from the interface.
1576  * @param pMI - msg interface
1577  * @returns NULL if no message received, otherwise a valid msg
1578  */
1579 static struct mt_msg *mt_msg_rx(struct mt_msg_interface *pMI)
1580 {
1581     int r;
1582     int nneed;
1583     struct mt_msg *pMsg;
1584
1585     /* do we allocate a new message? */
1586     if(pMI->pCurRxMsg == NULL)
1587     {
1588         pMI->pCurRxMsg = MT_MSG_alloc(-1, -1, -1);
1589         /* allocation error :-(*/
1590         if(pMI->pCurRxMsg == NULL)
1591         {
1592             return (NULL);
1593         }
1594         pMI->pCurRxMsg->pLogPrefix = _incomming_msg;
1595         MT_MSG_setSrcIface(pMI->pCurRxMsg, pMI);
1596     }
1597     else
1598     {
1599         MT_MSG_resetMsg(pMI->pCurRxMsg, -1, -1, -1);
1600     }
1601
1602     pMsg = pMI->pCurRxMsg;
1603
1604     LOG_printf(LOG_DBG_MT_MSG_traffic,
1605                "%s: rx-msg looking for start\n",
1606                pMI->dbg_name);
1607
1608 try_again:
1609     /* zap what we have */
1610     pMsg->iobuf_nvalid = 0;
1611     /* zap the existing buffer for debug reasons */
1612     memset((void *)(&(pMsg->iobuf[0])), 0, pMsg->iobuf_idx_max);
1613
1614     /* how many bytes should we get? */
1615     nneed = (
1616         (pMI->frame_sync ? 1 : 0) + /* sync */
1617         (pMI->len_2bytes ? 2 : 1) + /* len */
1618         1 + /* cmd0 */
1619         1 + /* cmd1 */
1620         0 + /* unknown length yet */
1621         (pMI->include_chksum ? 1 : 0)); /* checksum */
1622
1623 read_more:
1624     r = mt_msg_rx_bytes(pMI, nneed, pMI->intermsg_timeout_mSecs);
1625     if(r == 0)
1626     {
1627         LOG_printf(LOG_DBG_MT_MSG_traffic, "%s: rx-silent\n", pMI->dbg_name);
1628         return (NULL);
1629     }
1630
1631     if(r < 0)
1632     {
1633         /* something is wrong */
1634         LOG_printf(LOG_DBG_MT_MSG_traffic, "%s: Io error?\n", pMI->dbg_name);
1635         return (NULL);
1636     }
1637
1638     /* we start reading at byte 0 in the message */
1639     pMsg->iobuf_idx = 0;
1640
1641     /* should we find a frame sync? */
1642     if(pMI->frame_sync)
1643     {
1644         /* hunt for the sync byte */
1645         /* and move the sync byte to byte 0 in the buffer */
1646         uint8_t *p8;
1647
1648         p8 = (uint8_t *)memchr((void *)(&pMsg->iobuf[0]),
1649                                0xfe,
1650                                pMsg->iobuf_nvalid);
1651         if(p8 == NULL)
1652         {
1653             /* not found */
1654             MT_MSG_log(LOG_DBG_MT_MSG_traffic | LOG_DBG_MT_MSG_raw,
1655                        pMsg, "Garbage data...\n");
1656             goto try_again;
1657         }
1658
1659         /* frame sync must start at zero. */
1660         if(p8 != pMsg->iobuf)
1661         {
1662             /* need to shift data over some */
1663
1664             /* how many bytes to shift? */
1665             int n;
1666             n = (int)(&(pMsg->iobuf[pMsg->iobuf_nvalid]) - p8);
1667             /* shift */
1668             memmove((void *)(&pMsg->iobuf[0]), (void *)p8, n);
1669             /* zero what we deleted */
1670             memset((void *)(&(pMsg->iobuf[n])), 0, pMsg->iobuf_nvalid - n);
1671             pMsg->iobuf_nvalid = n;
1672             /* Do we have enough bytes? */
1673             if(nneed > pMsg->iobuf_nvalid)
1674             {
1675                 /* No - we need more, go get more */
1676                 goto read_more;
1677             }
1678         }
1679         /* DUMMY read of the sync byte */
1680         MT_MSG_rdU8(pMsg);
1681     }
1682
1683     /* Found start */
1684     /* how big is our length? */
1685     if(pMI->len_2bytes)
1686     {
1687         pMsg->expected_len = MT_MSG_rdU16(pMsg);
1688     }
1689     else
1690     {
1691         pMsg->expected_len = MT_MSG_rdU8(pMsg);
1692     }
1693
1694     pMsg->cmd0 = MT_MSG_rdU8(pMsg);
1695     pMsg->cmd1 = MT_MSG_rdU8(pMsg);
1696
1697     nneed += pMsg->expected_len;
1698
1699     /* read the data component */
1700     r = mt_msg_rx_bytes(pMI, nneed, pMI->intersymbol_timeout_mSecs);
1701     if(r != nneed)
1702     {
1703         /* something is wrong? */
1704         if(r > 0)
1705         {
1706             /* we got some ... but not enough .. */
1707             LOG_printf(LOG_DBG_MT_MSG_raw,
1708                 "Short read ... got: %d, want: %d, try again...\n",
1709                 nneed, r);
1710             goto read_more;
1711         }
1712         MT_MSG_log(LOG_ERROR, pMsg, "%s: expected: %d, got: %d\n",
1713             pMI->dbg_name, nneed, r);
1714     dump_recover:
1715         LOG_printf(LOG_DBG_MT_MSG_traffic, "Flushing RX stream\n");
1716         /* Dump all incomming data until we find a sync byte */
1717         STREAM_rdDump(pMI->hndl, pMI->flush_timeout_mSecs);
1718         goto try_again;
1719     }
1720
1721     /* Dummy read to the end of the data. */
1722     /* this puts us at the checksum byte (if present) */
1723     MT_MSG_rdBuf(pMsg, NULL, pMsg->expected_len);
1724
1725     /* do the checksum */
1726     if(pMI->include_chksum)
1727     {
1728         r = MT_MSG_calc_chksum(pMsg, 'f', pMsg->iobuf_nvalid);
1729         if(r != 0)
1730         {
1731             MT_MSG_log(LOG_ERROR, pMI->pCurRxMsg, "%s: chksum error\n",
1732                 pMI->dbg_name);
1733             LOG_hexdump(!LOG_ERROR, 0, pMsg->iobuf, pMsg->iobuf_nvalid);
1734             goto dump_recover;
1735         }
1736     }
1737     /* We have a message */
1738     MT_MSG_set_type(pMsg, pMsg->pSrcIface);
1739     /* Set the iobuf_idx to the start of the payload */
1740
1741     /* since we will be parsing the message... */
1742     /* Set the iobuf_idx to the start of the payload */
1743     pMsg->iobuf_idx = (
1744         (pMI->frame_sync ? 1 : 0) +
1745         (pMI->len_2bytes ? 2 : 1) +
1746         1 + /* cmd0 */
1747         1 /* cmd1 */
1748        );
1749     /* next time we need to allocate a new message */
1750     pMI->pCurRxMsg = NULL;
1751
1752     /* return our message */
1753     return (pMsg);
1754 }
1755
1756 /*!
1757  * @brief We have received an extended status message handle it
1758  * @param pMsg - the message
1759  * @return NULL if we are not done with the fragmentation
1760  */
1761 static struct mt_msg *handle_ext_status(struct mt_msg *pMsg)
1762 {
1763     int block_num;
1764     int status;
1765     const char *cp;
1766
1767     /* ignore the version byte */
1768     MT_MSG_rdU8(pMsg);
1769     block_num = MT_MSG_rdU8(pMsg);
1770     status = MT_MSG_rdU8(pMsg);
1771
1772     switch (status)
1773     {
1774     default:
1775         cp = "unknown";
1776         break;
1777     case MT_MSG_EXT_STATUS_mem_alloc_error:
1778         cp = "alloc-error";
1779         break;
1780     case MT_MSG_EXT_STATUS_frag_complete:
1781         cp = "frag-complete";
1782         break;
1783     case MT_MSG_EXT_STATUS_frag_aborted:
1784         cp = "aborted";
1785         break;
1786     case MT_MSG_EXT_STATUS_unsupported_ack:
1787         cp = "unsupported-ack";
1788         break;
1789     }
1790     MT_MSG_log(LOG_DBG_MT_MSG_traffic, pMsg, "extended status: block: %d, %s\n",
1791         block_num, cp);
1792     MT_MSG_free(pMsg);
1793     pMsg = NULL;
1794     return (NULL);
1795 }
1796
1797 /*!
1798  * @brief Send the fragmentation ack for blocknum.
1799  * @param pMI - where to send it
1800  * @param pFI - fragmentation info
1801  */
1802 static void send_frag_ack(struct mt_msg_interface *pMI,
1803                           struct mt_msg_iface_frag_info *pFI)
1804 {
1805     struct mt_msg *pAck;
1806
1807     /* create our extended packet */
1808     pAck = MT_MSG_alloc(3, pFI->pMsg->cmd0 | _bit7, pFI->pMsg->cmd1);
1809     if(pAck == NULL)
1810     {
1811         /* nothing we can do.. */
1812         return;
1813     }
1814     pAck->pLogPrefix = "frag-ack";
1815     MT_MSG_setDestIface(pAck, pMI);
1816     MT_MSG_wrU8(pAck, (3 << 3) | pMI->stack_id);
1817     MT_MSG_wrU8(pAck, pFI->block_cur);
1818
1819     if((pFI->block_cur+ 1) == pFI->block_count)
1820     {
1821         MT_MSG_wrU8(pAck, MT_MSG_FRAG_STATUS_frag_complete);
1822     }
1823     else
1824     {
1825         MT_MSG_wrU8(pAck, MT_MSG_FRAG_STATUS_success);
1826     }
1827     MT_MSG_tx_raw(pAck);
1828     MT_MSG_free(pAck);
1829 }
1830
1831 /*!
1832  * @brief handle first packet of a fragmented message
1833  * @param pRxmsg - the fragment we just received.
1834  * @return 0 success
1835  */
1836 static int rx_first_frag_block(struct mt_msg *pRxMsg)
1837 {
1838     struct mt_msg_interface *pMI;
1839     int rd_loc;
1840     int wr_loc;
1841
1842     pMI = pRxMsg->pSrcIface;
1843
1844     /* we don't use these */
1845     pMI->rx_frag.pTxFragAck = NULL;
1846     pMI->rx_frag.pTxFragData = NULL;
1847
1848     /* we have no error (YET!) */
1849     pMI->rx_frag.is_error = false;
1850
1851     /* we'll keep this here. */
1852     pMI->rx_frag.pMsg = pRxMsg;
1853
1854     /* skip the version byte */
1855     MT_MSG_rdU8(pMI->rx_frag.pMsg);
1856
1857     /* read the block number */
1858     /* must start with block 0 */
1859     pMI->rx_frag.block_cur = MT_MSG_rdU8(pMI->rx_frag.pMsg);
1860
1861     pMI->rx_frag.total_size = MT_MSG_rdU16(pMI->rx_frag.pMsg);
1862
1863     /* "-4" is because the size of *this* extended header is 4 bytes. */
1864     pMI->rx_frag.this_frag_size = pMI->rx_frag.pMsg->expected_len - 4;
1865
1866     /* determine how many blocks we should receive */
1867     pMI->rx_frag.block_count =
1868         (pMI->rx_frag.total_size + pMI->rx_frag.this_frag_size - 1) /
1869         pMI->rx_frag.this_frag_size;
1870
1871     MT_MSG_log(LOG_DBG_MT_MSG_traffic,
1872         pMI->rx_frag.pMsg, "RX Frag: Block %d of %d, frag size: %d\n",
1873         pMI->rx_frag.block_cur + 1,
1874         pMI->rx_frag.block_count,
1875         pMI->rx_frag.this_frag_size);
1876
1877     /* sanity check, did we get block 0? */
1878     if(pMI->rx_frag.block_cur != 0)
1879     {
1880         MT_MSG_log(LOG_ERROR, pMI->rx_frag.pMsg, "RX-non-first-block\n");
1881
1882         /* abort */
1883         send_frag_abort_outoforder(pMI, &(pMI->rx_frag));
1884
1885         /* toss the message we just received */
1886         MT_MSG_free(pMI->rx_frag.pMsg);
1887         pMI->rx_frag.pMsg = NULL;
1888         /* we handled ZERO messages */
1889         return (0);
1890     }
1891
1892     /* Now, move the data in the message to the front */
1893     /* we are going to throw away the extended header */
1894
1895     /* this is where the payload should be when done */
1896     wr_loc =
1897         (pMI->frame_sync ? 1 : 0) +
1898         (pMI->len_2bytes ? 2 : 1) +
1899         1 + /* cmd0 */
1900         1; /* cmd1 */
1901
1902     /* the payload currently is after the header */
1903     rd_loc = wr_loc + 4;
1904
1905     /* now move the data */
1906     memmove((void *)(&(pMI->rx_frag.pMsg->iobuf[wr_loc])),
1907         (void *)(&(pMI->rx_frag.pMsg->iobuf[rd_loc])),
1908         pMI->rx_frag.this_frag_size);
1909
1910     /* update the expected size */
1911     pMI->rx_frag.pMsg->expected_len = pMI->rx_frag.total_size;
1912     /* clear the extended bit */
1913     pMI->rx_frag.pMsg->cmd0 &= 0x7f;
1914
1915     /* send our ack. */
1916     send_frag_ack(pMI, &(pMI->rx_frag));
1917     /* we successfully handled 1 message */
1918     return (1);
1919 }
1920
1921 /*!
1922  * @brief We have received an extended packet of type fragment data.
1923  * @param pMI - where it came from
1924  * @param pRxFrag - what we received.
1925  */
1926 static struct mt_msg *handle_data_fragment(struct mt_msg *pRxFrag)
1927 {
1928     struct mt_msg_interface *pMI;
1929     int this_block;
1930     int this_len;
1931     int rd_loc;
1932     int wr_loc;
1933     bool last_block;
1934     bool bad;
1935
1936     /* recover the interface */
1937     pMI = pRxFrag->pSrcIface;
1938
1939     /* first packet is special. */
1940     if(pMI->rx_frag.pMsg == NULL)
1941     {
1942         rx_first_frag_block(pRxFrag);
1943         return (NULL);
1944     }
1945
1946     /* all others are handled here. */
1947
1948     /* throw away the 1st byte */
1949     MT_MSG_rdU8(pRxFrag);
1950     this_block = MT_MSG_rdU8(pRxFrag);
1951     this_len   = MT_MSG_rdU16(pRxFrag);
1952
1953     /* detect errors and cleanup */
1954     if(pMI->rx_frag.block_cur == this_block)
1955     {
1956         MT_MSG_log(LOG_DBG_MT_MSG_traffic,
1957                    pRxFrag,
1958                    "RX Frag: Duplicate block %d\n",
1959                    this_block);
1960     abort_clean_up:
1961         if(pMI->rx_frag.pMsg)
1962         {
1963             MT_MSG_free(pMI->rx_frag.pMsg);
1964             pMI->rx_frag.pMsg = NULL;
1965         }
1966         if(pRxFrag)
1967         {
1968             MT_MSG_free(pRxFrag);
1969             pRxFrag = NULL;
1970         }
1971         return (NULL);
1972     }
1973
1974     if(pMI->rx_frag.total_size != this_len)
1975     {
1976         MT_MSG_log(LOG_ERROR, pRxFrag,
1977                    "RX Frag: total size change (was: %d, now: %d)\n",
1978                    pMI->rx_frag.total_size, this_len);
1979         send_frag_abort_outoforder(pMI, &(pMI->rx_frag));
1980         goto abort_clean_up;
1981     }
1982
1983     /* is this the last block? */
1984     last_block = false;
1985     if((this_block + 1) == pMI->rx_frag.block_count)
1986     {
1987         last_block = true;
1988     }
1989
1990     /* block order wrong? */
1991     if((pMI->rx_frag.block_cur + 1) != this_block)
1992     {
1993         MT_MSG_log(LOG_ERROR, pRxFrag,
1994                    "RX Frag: out of order, expect %d, got %d\n",
1995                    (pMI->rx_frag.block_cur + 1), this_block);
1996         send_frag_abort_outoforder(pMI, &(pMI->rx_frag));
1997         goto abort_clean_up;
1998     }
1999
2000     /* how big is this specific fragment? */
2001     this_len = pRxFrag->expected_len - 4;
2002
2003     /* size must not change */
2004     bad = false;
2005     if(last_block)
2006     {
2007         /* last block can be smaller, but not larger */
2008         bad = (this_len > pMI->rx_frag.this_frag_size);
2009     }
2010     else
2011     {
2012         /* internal blocks must be same size */
2013         bad = (this_len != pMI->rx_frag.this_frag_size);
2014     }
2015
2016     if(bad)
2017     {
2018         /* it changed, this is wrong, so abort */
2019         MT_MSG_log(LOG_ERROR,
2020                    pRxFrag,
2021                    "RX Frag: block len change new: %d, old: %d\n",
2022                    this_len, pMI->rx_frag.this_frag_size);
2023         send_frag_ack_packet(pMI,
2024                              &(pMI->rx_frag),
2025                              MT_MSG_FRAG_STATUS_block_len_changed);
2026         goto abort_clean_up;
2027     }
2028
2029     /* update the block number. */
2030     pMI->rx_frag.block_cur = this_block;
2031
2032     MT_MSG_log(LOG_DBG_MT_MSG_traffic,
2033         pRxFrag,
2034         "RX-Frag: Block %d of %d\n",
2035         this_block + 1,
2036         pMI->rx_frag.block_count);
2037
2038     /* otherwise we are good, copy the data */
2039     rd_loc =
2040         4 + /* go past the extended header */
2041         (pMI->frame_sync ? 1 : 0) +
2042         (pMI->len_2bytes ? 2 : 1) +
2043         1 + /* cmd0 */
2044         1; /* cmd1; */
2045
2046     /* where do we put it? */
2047     wr_loc = pMI->rx_frag.block_cur * pMI->rx_frag.this_frag_size;
2048     /* go past the header in the 'whole' packet. */
2049     wr_loc +=
2050         (pMI->frame_sync ? 1 : 0) +
2051         (pMI->len_2bytes ? 2 : 1) +
2052         1 + /* cmd0 */
2053         1; /* cmd1 */
2054
2055     /* copy the data. */
2056     memcpy((void *)(&(pMI->rx_frag.pMsg->iobuf[wr_loc])),
2057         (void *)(&(pRxFrag->iobuf[rd_loc])),
2058         this_len);
2059
2060     /* send our ack */
2061     send_frag_ack(pMI, &(pMI->rx_frag));
2062
2063     /* we no longer need the fragment */
2064     MT_MSG_free(pRxFrag);
2065     pRxFrag = NULL;
2066
2067     struct mt_msg *pWhole;
2068     pWhole = NULL;
2069     if(last_block)
2070     {
2071         /* send COMPLETE */
2072         send_extended_status(pMI,
2073                              &(pMI->rx_frag),
2074                              MT_MSG_EXT_STATUS_frag_complete);
2075
2076         pWhole = pMI->rx_frag.pMsg;
2077         pMI->rx_frag.pMsg = NULL;
2078
2079         /* set the parse point. */
2080         pWhole->iobuf_idx =
2081             (pMI->frame_sync ? 1 : 0) +
2082             (pMI->len_2bytes ? 2 : 1) +
2083             1 + /* cmd0 */
2084             1; /* cmd1 */
2085
2086     }
2087     return (pWhole);
2088 }
2089
2090 /*!
2091  * @brief We have received an extended packet, handle it
2092  * @param pMsg- the packet we received.
2093  */
2094 static struct mt_msg *handle_extend_packet(struct mt_msg *pMsg)
2095 {
2096     struct mt_msg_interface *pMI;
2097     const char *cp;
2098     /* recover where it came from */
2099
2100     pMI = pMsg->pSrcIface;
2101
2102     cp = NULL;
2103     switch (pMsg->m_type)
2104     {
2105     default:
2106         pMsg = NULL;
2107         BUG_HERE("invalid msg type\n");
2108         break;
2109     case MT_MSG_TYPE_sreq_frag_ack:
2110         if(cp == NULL) cp = "sreq_frag_ack";
2111         /* fallthru; */
2112     case MT_MSG_TYPE_areq_frag_ack:
2113         if(cp == NULL) cp = "areq_frag_ack";
2114         /* fallthru; */
2115     case MT_MSG_TYPE_srsp_frag_ack:
2116         if(cp == NULL) cp = "srsp_frag_ack";
2117         /* let sender deal with this. */
2118         pMsg->pLogPrefix = cp;
2119         MT_MSG_log(LOG_DBG_MT_MSG_traffic, pMsg, "RX frag-ack\n");
2120
2121         MUTEX_lock(pMI->list_lock, -1);
2122         pMsg->pListNext = pMI->tx_frag.pTxFragAck;
2123         pMI->tx_frag.pTxFragAck = pMsg;
2124         pMsg = NULL;
2125         MUTEX_unLock(pMI->list_lock);
2126         SEMAPHORE_put(pMI->tx_frag.tx_ack_semaphore);
2127         break;
2128     case MT_MSG_TYPE_sreq_frag_data:
2129         if(cp == NULL) cp = "sreq_frag_data";
2130         /* fallthrugh */
2131     case MT_MSG_TYPE_areq_frag_data:
2132         if(cp == NULL) cp = "areq_frag_data";
2133         /* fallthrugh */
2134     case MT_MSG_TYPE_srsp_frag_data:
2135         if(cp == NULL) cp = "srsp_frag_data";
2136         pMsg->pLogPrefix = cp;
2137         pMsg = handle_data_fragment(pMsg);
2138         break;
2139     case MT_MSG_TYPE_sreq_ext_status:
2140         if(cp == NULL) cp = "sreq_ext_status";
2141         /* fallthru; */
2142     case MT_MSG_TYPE_areq_ext_status:
2143         if(cp == NULL) cp = "areq_ext_status";
2144         /* fallthru; */
2145     case MT_MSG_TYPE_srsp_ext_status:
2146         if(cp == NULL) cp = "srsp_ext_status";
2147         pMsg->pLogPrefix = cp;
2148         pMsg = handle_ext_status(pMsg);
2149         break;
2150     }
2151     return (pMsg);
2152 }
2153
2154 /*!
2155  * @brief rx thread that handles all incomming messages.
2156  * @param cookie - the message interface in disguise
2157  * @return nothing important.
2158  */
2159 static intptr_t mt_msg_rx_thread(intptr_t cookie)
2160 {
2161     int a;
2162     int b;
2163     struct mt_msg_interface *pMI;
2164     struct mt_msg *pRxMsg;
2165
2166     /* recover our message */
2167     pMI = (struct mt_msg_interface *)(cookie);
2168     /* run till we die */
2169     for(;;)
2170     {
2171         if(pMI->is_dead)
2172         {
2173             break;
2174         }
2175
2176         if(STREAM_isError(pMI->hndl))
2177         {
2178             LOG_printf(LOG_ERROR, "%s: Dead\n", pMI->dbg_name);
2179             break;
2180         }
2181
2182         /* rx a message (this is a blocking call) */
2183         pRxMsg = mt_msg_rx(pMI);
2184         if(pRxMsg == NULL)
2185         {
2186             continue;
2187         }
2188         /* Debug dump if requested */
2189         MT_MSG_dbg_decode(pRxMsg, pRxMsg->pSrcIface, ALL_MT_MSG_DBG);
2190
2191         /* if this message has the extension bit.. */
2192         if(pRxMsg->cmd0 & _bit7)
2193         {
2194             pRxMsg = handle_extend_packet(pRxMsg);
2195         }
2196
2197         /* did we complete the decoding of a the extended packet? */
2198         /* or if we got a normall packet... */
2199         if(pRxMsg == NULL)
2200         {
2201             /* nothing left to do... */
2202             continue;
2203         }
2204
2205         if(pRxMsg->m_type == MT_MSG_TYPE_areq)
2206         {
2207         areq_msg:
2208             MT_MSG_log(LOG_DBG_MT_MSG_traffic, pRxMsg, "rx areq\n");
2209             /* async request */
2210             MT_MSG_LIST_insert(pMI, &(pMI->rx_list), pRxMsg);
2211             pRxMsg = NULL;
2212             continue;
2213         }
2214
2215         if(pRxMsg->m_type == MT_MSG_TYPE_poll)
2216         {
2217             /* polls are handled as an areq */
2218             goto areq_msg;
2219         }
2220
2221         if(pRxMsg->m_type == MT_MSG_TYPE_sreq)
2222         {
2223             /* polls are handled as an areq */
2224             goto areq_msg;
2225         }
2226
2227         /* it should match our current Sreq */
2228         /* and it might not match our Sreq */
2229         if(pMI->pCurSreq == NULL)
2230         {
2231             /* But there is no current sreq? */
2232             MT_MSG_log(LOG_DBG_MT_MSG_traffic, pRxMsg, "no pending sreq?\n");
2233             /* treat as an areq */
2234             goto areq_msg;
2235         }
2236
2237         /* Does this match? */
2238
2239         /* Upper bits[7:5] = message type */
2240         /* Lower bits[4:0] = subsystem number */
2241         /* We only care about the subsystem number */
2242         a = _bitsXYof(pMI->pCurSreq->cmd0, 4, 0);
2243         b = _bitsXYof(pRxMsg->cmd0, 4, 0);
2244         if((a == b) && (pMI->pCurSreq->cmd1 == pRxMsg->cmd1))
2245         {
2246             /* All is well */
2247         }
2248         else
2249         {
2250             /* does not match.. */
2251             MT_MSG_log(LOG_DBG_MT_MSG_traffic,
2252                 pRxMsg,
2253                 "sreq(cmd0=0x%02x, cmd1=0x%02x) does not match\n",
2254                 pMI->pCurSreq->cmd0,
2255                 pMI->pCurSreq->cmd1);
2256             /* treat as areq */
2257             goto areq_msg;
2258         }
2259
2260         /* attach it to the request */
2261         pMI->pCurSreq->pSrsp = pRxMsg;
2262         pMI->pCurSreq = NULL;
2263         pRxMsg = NULL;
2264         /* wake up the waiter */
2265         SEMAPHORE_put(pMI->srsp_semaphore);
2266     }
2267     LOG_printf(LOG_ERROR, "%s: rx-thread dead\n", pMI->dbg_name);
2268     /* we die */
2269     return (0);
2270 }
2271
2272 /*
2273   Initialize a message interface.
2274   see mt_msg.h
2275 */
2276 int MT_MSG_interfaceCreate(struct mt_msg_interface *pMI)
2277 {
2278     int r;
2279
2280     pMI->is_dead = false;
2281
2282     /*
2283      The handle may come in pre-populated.
2284      or we may need to create our socket interface
2285     */
2286     if(pMI->s_cfg)
2287     {
2288         if(pMI->s_cfg->ascp == 's')
2289         {
2290             /*
2291               you really need to do this instead:
2292                (A) create the socket.
2293                (B) set socket to listen mode.
2294                (C) then accept socket connections
2295                (D) For each accepted socket...
2296                      Possibly create new threads for each socket.
2297                      That is not something we do here.
2298             */
2299             BUG_HERE("server socket is not supported here\n");
2300         }
2301         else
2302         {
2303             /* create our socket */
2304             pMI->hndl = SOCKET_CLIENT_create(pMI->s_cfg);
2305             if(pMI->hndl)
2306             {
2307                 /* then connect */
2308                 r = SOCKET_CLIENT_connect(pMI->hndl);
2309                 if(r < 0)
2310                 {
2311                     /* destroy if we could not connect */
2312                     SOCKET_CLIENT_destroy(pMI->hndl);
2313                     pMI->hndl = 0;
2314                 }
2315             }
2316         }
2317     }
2318     else if(pMI->u_cfg)
2319     {
2320         pMI->hndl = STREAM_createUart(pMI->u_cfg);
2321         if(pMI->hndl)
2322         {
2323             /* go flush all incomming data */
2324             if( pMI->startup_flush )
2325                 STREAM_rdDump(pMI->hndl, pMI->flush_timeout_mSecs);
2326         }
2327     }
2328     else
2329     {
2330         /* Connection is a *SOCKET* */
2331         /* Verify we have a socket handle! */
2332         if(pMI->hndl == 0)
2333         {
2334             BUG_HERE("no interface pointer\n");
2335         }
2336     }
2337
2338     if(pMI->hndl == 0)
2339     {
2340         goto bad;
2341     }
2342
2343     r= MT_MSG_LIST_create(&(pMI->rx_list), pMI->dbg_name, "rx-msgs");
2344     if(r != 0)
2345     {
2346         goto bad;
2347     }
2348
2349     pMI->tx_lock = MUTEX_create("mi-tx-lock");
2350     pMI->srsp_semaphore = SEMAPHORE_create("srsp-semaphore", 0);
2351     pMI->tx_frag.tx_ack_semaphore = SEMAPHORE_create("frag-semaphore", 0);
2352     pMI->list_lock = MUTEX_create("mi-lock");
2353
2354     if((pMI->tx_lock == 0) ||
2355         (pMI->srsp_semaphore == 0) ||
2356         (pMI->tx_frag.tx_ack_semaphore == 0) ||
2357         (pMI->list_lock == 0))
2358     {
2359         goto bad;
2360     }
2361
2362     /* attempt to choose reasonable defaults */
2363     if(pMI->tx_frag_size == 0)
2364     {
2365         /*
2366             1 byte frame sync [optional]
2367             1 byte len
2368             1 byte cmd0
2369             1 byte cmd1
2370             ext: 1 byte version
2371             ext: 2 byte total length
2372             ext: 1 byte block number
2373              *variable* data
2374             1 byte checksum [optional]
2375            =======
2376             9 bytes allocated, worse case
2377            256 - 9 = 247
2378         */
2379         pMI->tx_frag_size = 247;
2380     }
2381
2382     if(pMI->retry_max == 0)
2383     {
2384         pMI->retry_max = 3;
2385     }
2386
2387     if(pMI->frag_timeout_mSecs == 0)
2388     {
2389         pMI->frag_timeout_mSecs = 2000;
2390     }
2391
2392     if(pMI->intersymbol_timeout_mSecs == 0)
2393     {
2394         pMI->intersymbol_timeout_mSecs = 100;
2395     }
2396
2397     if(pMI->srsp_timeout_mSecs == 0)
2398     {
2399         pMI->srsp_timeout_mSecs = 3000;
2400     }
2401
2402     if(pMI->flush_timeout_mSecs == 0)
2403     {
2404         pMI->flush_timeout_mSecs = 50;
2405     }
2406
2407     if(pMI->intermsg_timeout_mSecs == 0)
2408     {
2409         pMI->intermsg_timeout_mSecs = 3000;
2410     }
2411
2412     if(pMI->tx_lock_timeout == 0)
2413     {
2414         pMI->tx_lock_timeout = 3000;
2415     }
2416
2417     /* create the thread last... because it is going to run */
2418     pMI->rx_thread = THREAD_create(pMI->dbg_name,
2419                                     mt_msg_rx_thread,
2420                                     (intptr_t)(pMI),
2421                                     THREAD_FLAGS_DEFAULT);
2422
2423     if((pMI->hndl == 0) || (pMI->rx_thread == 0))
2424     {
2425     bad:
2426         /* problem? */
2427         MT_MSG_interfaceDestroy(pMI);
2428         return (-1);
2429     }
2430     else
2431     {
2432         return (0);
2433     }
2434 }
2435
2436 /*
2437   Create a message list
2438   see mt_msg.h
2439 */
2440 int MT_MSG_LIST_create(struct mt_msg_list *pML,
2441                         const char *dbg_name,
2442                         const char *name2)
2443 {
2444     size_t len;
2445     char *cp;
2446
2447     memset((void *)(pML), 0, sizeof(*pML));
2448
2449     /* +1 - for null byte
2450      * +1 - for "-" in %s-%s" below
2451      * total 2
2452      */
2453     len = 2 + strlen(dbg_name);
2454     if(name2)
2455     {
2456         len += strlen(name2);
2457     }
2458     else
2459     {
2460         /* nothing to add */
2461     }
2462     cp = calloc(1, len);
2463     if(cp)
2464     {
2465         if(name2)
2466         {
2467             (void)snprintf(cp, len, "%s-%s", dbg_name, name2);
2468         }
2469         else
2470         {
2471             strcpy(cp, dbg_name);
2472         }
2473         pML->dbg_name = cp;
2474     }
2475     pML->sem = SEMAPHORE_create(dbg_name, 0);
2476     pML->pList = NULL;
2477
2478     if((pML->dbg_name == NULL) ||
2479         (pML->sem == 0))
2480     {
2481         MT_MSG_LIST_destroy(pML);
2482         return (-1);
2483     }
2484     else
2485     {
2486         return (0);
2487     }
2488 }
2489
2490 /*
2491   Insert a message into this message list.
2492   see mt_msg.h
2493 */
2494 void MT_MSG_LIST_insert(struct mt_msg_interface *pMI,
2495                          struct mt_msg_list *pML,
2496                          struct mt_msg *pMsg)
2497 {
2498     struct mt_msg **ppMsg;
2499
2500     MUTEX_lock(pMI->list_lock,-1);
2501     ppMsg = &(pML->pList);
2502
2503     /* nothing follows this guy */
2504     pMsg->pListNext = NULL;
2505
2506     /* goto end of the list */
2507     while(*ppMsg)
2508     {
2509         ppMsg = &((*ppMsg)->pListNext);
2510     }
2511
2512     /* add to end */
2513     *ppMsg = pMsg;
2514
2515     MUTEX_unLock(pMI->list_lock);
2516
2517     SEMAPHORE_put(pML->sem);
2518 }
2519
2520 /*
2521   Remove a message from this message list.
2522   see mt_msg.h
2523 */
2524 struct mt_msg *MT_MSG_LIST_remove(struct mt_msg_interface *pMI,
2525                             struct mt_msg_list *pML, int timeout_mSecs)
2526 {
2527     struct mt_msg *pMsg;
2528
2529     /* did data arrive? */
2530     SEMAPHORE_waitWithTimeout(pML->sem, timeout_mSecs);
2531
2532     /* remove */
2533     MUTEX_lock(pMI->list_lock, -1);
2534
2535     pMsg = pML->pList;
2536     if(pMsg)
2537     {
2538         pML->pList = pMsg->pListNext;
2539         pMsg->pListNext = NULL;
2540     }
2541
2542     MUTEX_unLock(pMI->list_lock);
2543
2544     return (pMsg);
2545 }
2546
2547 /*
2548   Destroy a message list
2549   see mt_msg.h
2550 */
2551 void MT_MSG_LIST_destroy(struct mt_msg_list *pML)
2552 {
2553     struct mt_msg *pMsg;
2554
2555     /* the list might be in a strange state */
2556     /* not fully initialized */
2557     /* do this carefully */
2558
2559     while(pML->pList)
2560     {
2561         pMsg = pML->pList;
2562         pML->pList = pMsg->pListNext;
2563         pMsg->pListNext = NULL;
2564
2565         MT_MSG_free(pMsg);
2566     }
2567
2568     if(pML->sem)
2569     {
2570         SEMAPHORE_destroy(pML->sem);
2571         pML->sem = 0;
2572     }
2573
2574     if(pML->dbg_name)
2575     {
2576         free_const((const void *)(pML->dbg_name));
2577         pML->dbg_name = NULL;
2578     }
2579
2580     memset((void *)(pML), 0, sizeof(*pML));
2581
2582 }
2583
2584 /*
2585  Transmit this message, and if needed receive the srsp reply
2586   see mt_msg.h
2587 */
2588 int MT_MSG_txrx(struct mt_msg *pMsg)
2589 {
2590     int r;
2591     struct mt_msg_interface *pMI;
2592
2593     /* get our destination interface */
2594     pMI = pMsg->pDestIface;
2595
2596     /* do not transmit 2 messages at the same time */
2597     r = MUTEX_lock(pMI->tx_lock, pMI->tx_lock_timeout);
2598     if(r != 0)
2599     {
2600         LOG_printf(LOG_ERROR, "%s: Interface lock timeout\n", pMI->dbg_name);
2601         MT_MSG_log(LOG_ERROR, pMsg, "Interface lock timeout\n");
2602         /* we transmitted zero messages */
2603         return (0);
2604     }
2605
2606     /* We have no response yet */
2607     pMsg->pSrsp = NULL;
2608
2609     MT_MSG_set_type(pMsg, pMI);
2610
2611     if(pMsg->m_type == MT_MSG_TYPE_unknown)
2612     {
2613         BUG_HERE("unknown msg type\n");
2614     }
2615
2616     /* we are about to send an SREQ */
2617     if(pMI->pCurSreq)
2618     {
2619         /* There should *not* be any pending
2620          * or left over SREQ from before...
2621          * if there is one, it is a bug. */
2622         BUG_HERE("interface: %s, has a pending SREQ!\n", pMI->dbg_name);
2623     }
2624
2625     /* this is our pending SREQ... */
2626     pMI->pCurSreq = pMsg;
2627     /* send our message */
2628     r = MT_MSG_tx(pMsg);
2629
2630     /* could we send it? */
2631     if(r != 1)
2632     {
2633         MT_MSG_log(LOG_ERROR,
2634                    pMsg,
2635                    "Cannot transmit, result: %d (expected: 1)\n", r);
2636
2637         /* No, ... cleanup */
2638         /* we have nothing pending any more. */
2639         pMI->pCurSreq = NULL;
2640         pMsg->is_error = true;
2641         /* did not transmit and did not receive */
2642         r = 0;
2643         goto done;
2644     }
2645
2646     /* we transmitted, so our result so far is 1. */
2647     r = 1;
2648     /* is this a command expecting a response? */
2649     if(pMsg->m_type != MT_MSG_TYPE_sreq)
2650     {
2651         /* we have nothing pending any more. */
2652         pMI->pCurSreq = NULL;
2653     }
2654     else
2655     {
2656         /* wait for the response... */
2657         SEMAPHORE_waitWithTimeout(pMI->srsp_semaphore, pMI->srsp_timeout_mSecs);
2658         /* clear the SREQ  */
2659         pMI->pCurSreq = NULL;
2660         /* Did we get our answer?  */
2661         if(pMsg->pSrsp)
2662         {
2663             /* Yea!! Success! */
2664             /* we received +1 */
2665             r = r + 1;
2666             /* --- Total =2 */
2667         }
2668     }
2669 done:
2670     MUTEX_unLock(pMI->tx_lock);
2671     return (r);
2672 }
2673
2674 /*
2675   mark this cmd0 byte as a poll.
2676   Public function mt_msg.h
2677 */
2678 uint8_t MT_MSG_cmd0_poll(int cmd0)
2679 {
2680     return ((0 << 5) | _bitsXYof(cmd0, 4, 0));
2681 }
2682
2683 /*
2684   mark this cmd0 byte as sreq
2685   Public function mt_msg.h
2686 */
2687 uint8_t MT_MSG_cmd0_sreq(int cmd0)
2688 {
2689     return ((1 << 5) | _bitsXYof(cmd0, 4, 0));
2690 }
2691
2692 /*
2693   mark this cmd0 byte as an areq
2694   Public function mt_msg.h
2695 */
2696 uint8_t MT_MSG_cmd0_areq(int cmd0)
2697 {
2698     return ((2 << 5) | _bitsXYof(cmd0, 4, 0));
2699 }
2700
2701 /*
2702   mark this cmd0 byte as an srsp
2703   Public function mt_msg.h
2704 */
2705 uint8_t MT_MSG_cmd0_srsp(int cmd0)
2706 {
2707     return ((3 << 5) | _bitsXYof(cmd0, 4, 0));
2708 }
2709
2710 /*
2711  Set the destination interface
2712  Public function in mt_msg.h
2713 */
2714 void MT_MSG_setDestIface(struct mt_msg *pMsg, struct mt_msg_interface *pIface)
2715 {
2716     pMsg->pDestIface = pIface;
2717     /* once the destination interface is known */
2718     /* we can initialize the write index.      */
2719     init_wr_idx(pMsg);
2720 }
2721
2722 /*
2723  Set the source interface
2724  Public function in mt_msg.h
2725 */
2726 void MT_MSG_setSrcIface(struct mt_msg *pMsg, struct mt_msg_interface *pIface)
2727 {
2728     pMsg->pSrcIface = pIface;
2729 }
2730
2731 /*
2732   Reset so we can reuse the message
2733   Public function mt_msg.h
2734 */
2735 void MT_MSG_reset(struct mt_msg_interface *pIface, int type)
2736 {
2737     struct mt_msg *pMsg;
2738     int r;
2739
2740     /* allocate the message */
2741     pMsg = MT_MSG_alloc(1, 0x21, 0x01);
2742     if(pMsg == NULL)
2743     {
2744         return;
2745     }
2746
2747     pMsg->pLogPrefix = "reset-cmd";
2748
2749     MT_MSG_setDestIface(pMsg, pIface);
2750     /* set type */
2751     MT_MSG_wrU8(pMsg, (uint8_t)(type));
2752
2753     /* send & receive */
2754     r = MT_MSG_txrx(pMsg);
2755     (void)r;
2756     /* done */
2757     MT_MSG_free(pMsg);
2758 }
2759
2760 /*!
2761  * @brief internal function to get an address from the device.
2762  * @param pMI - the interface
2763  * @param typecode - the type of address to get
2764  * @param result - where to put it
2765  *
2766  * @return 2 (txmsg + rxmsg = 2 total msgs) on success
2767  */
2768 static int get_extAddr(struct mt_msg_interface *pMI,
2769                        int typecode,
2770                        uint8_t *result)
2771 {
2772     struct mt_msg *pMsg;
2773     struct mt_msg *pReply;
2774     int r;
2775     int x;
2776     int v;
2777
2778     /* not requested so we are done */
2779     if(result == NULL)
2780     {
2781         return (0);
2782     }
2783
2784     *result = 0;
2785
2786     /* allocate message */
2787     pMsg = MT_MSG_alloc(1, 0x27, 0xee);
2788     if(!pMsg)
2789     {
2790         return (0);
2791     }
2792     pMsg->pLogPrefix = "get-ext-addr";
2793     MT_MSG_setDestIface(pMsg, pMI);
2794
2795     pMsg->pLogPrefix = "get-ext-addr";
2796     /* specify what we want */
2797     MT_MSG_wrU8(pMsg, (uint8_t)(typecode));
2798
2799     /* get the result */
2800     r = MT_MSG_txrx(pMsg);
2801
2802     /* did we transfer 2 messages? */
2803     if(r == 2)
2804     {
2805         pReply = pMsg->pSrsp;
2806         /* Good, decode result */
2807         /* get type */
2808         v = MT_MSG_rdU8(pReply);
2809         if(v != typecode)
2810         {
2811             /* something is wrong */
2812             MT_MSG_log(LOG_ERROR, pReply,
2813                        "Invalid ext-addr type code: 0x%02x\n",
2814                        (unsigned)(v));
2815         }
2816         for( x = 0 ; x < 8 ; x++){
2817             result[x] = MT_MSG_rdU8(pReply);
2818         }
2819
2820         /* check at end */
2821         MT_MSG_parseComplete(pReply);
2822         if(pReply->is_error)
2823         {
2824             MT_MSG_log(LOG_ERROR, pReply, "Get ExtFailed\n");
2825             r = 0;
2826         }
2827         else
2828         {
2829             /* success! */
2830         }
2831     }
2832     MT_MSG_free(pMsg);
2833     return (r);
2834 }
2835
2836 /*
2837   Get an any number of addresses from the device
2838   Public function in mt_msg.h
2839 */
2840 int MT_MSG_getExtAddress(struct mt_msg_interface *pIface,
2841                          uint8_t *pib,
2842                          uint8_t *primary,
2843                          uint8_t *usr_cfg)
2844 {
2845     int r;
2846     r = 0;
2847     r += get_extAddr(pIface, 0, pib);
2848     r += get_extAddr(pIface, 1, primary);
2849     r += get_extAddr(pIface, 2, usr_cfg);
2850     return (r);
2851 }
2852
2853 /*
2854   Request software version of the device
2855   Public function in mt_msg.h
2856 */
2857 int MT_MSG_getVersion(struct mt_msg_interface *pIface,
2858                       struct mt_version_info *pInfo)
2859 {
2860     struct mt_msg *pMsg;
2861     int r;
2862
2863     if(pInfo)
2864     {
2865         memset((void *)(pInfo), 0, sizeof(*pInfo));
2866     }
2867     pMsg = MT_MSG_alloc(0, 0x21, 0x02);
2868     if(!pMsg)
2869     {
2870         return (0);
2871     }
2872     pMsg->pLogPrefix = "get-version";
2873     MT_MSG_setDestIface(pMsg, pIface);
2874     pMsg->pLogPrefix = "get-version";
2875     /* no data */
2876     if(pInfo)
2877     {
2878         memset((void *)(pInfo), 0, sizeof(*pInfo));
2879     }
2880     r = MT_MSG_txrx(pMsg);
2881     if(r != 2)
2882     {
2883         MT_MSG_free(pMsg);
2884         pMsg = NULL;
2885         return (r);
2886     }
2887
2888     if(pInfo)
2889     {
2890         pInfo->transport = MT_MSG_rdU8(pMsg->pSrsp);
2891         pInfo->product = MT_MSG_rdU8(pMsg->pSrsp);
2892         pInfo->major = MT_MSG_rdU8(pMsg->pSrsp);
2893         pInfo->minor = MT_MSG_rdU8(pMsg->pSrsp);
2894         pInfo->maint = MT_MSG_rdU8(pMsg->pSrsp);
2895         MT_MSG_parseComplete(pMsg->pSrsp);
2896
2897         if(pMsg->pSrsp->is_error)
2898         {
2899             /* we did not properly transfer 2 messages */
2900             r = 0;
2901             memset((void *)(pInfo), 0, sizeof(*pInfo));
2902         }
2903     }
2904     MT_MSG_free(pMsg);
2905     return (r);
2906 }
2907
2908 /*
2909   Perform a loop back test with the device
2910   Public function defined in mt_msg.h
2911 */
2912 int MT_MSG_loopback(struct mt_msg_interface *pIface, int repeatCount,
2913     uint32_t mSec_rate, size_t length, const uint8_t *pPayload)
2914
2915 {
2916     const uint8_t *pReply;
2917     struct mt_msg *pMsg;
2918     int r;
2919     int x;
2920
2921     pMsg = MT_MSG_alloc(1 + 4 + ((int)length), 0x27, 0x10);
2922     if(pMsg == NULL)
2923     {
2924         return (0);
2925     }
2926     pMsg->pLogPrefix = "loopback";
2927     MT_MSG_setDestIface(pMsg, pIface);
2928     MT_MSG_wrU8(pMsg, repeatCount);
2929     MT_MSG_wrU32(pMsg, mSec_rate);
2930     MT_MSG_wrBuf(pMsg, pPayload, length);
2931
2932     r = MT_MSG_txrx(pMsg);
2933     if(r == 2)
2934     {
2935         /* we got our reply! */
2936         /* we don't care about the N repeats */
2937         MT_MSG_rdU8(pMsg->pSrsp);
2938         /* we don't care about the time  */
2939         MT_MSG_rdU32(pMsg->pSrsp);
2940         pReply = &(pMsg->pSrsp->iobuf[pMsg->pSrsp->iobuf_idx]);
2941         MT_MSG_rdBuf(pMsg->pSrsp, NULL, length);
2942         MT_MSG_parseComplete(pMsg->pSrsp);
2943
2944         if(pMsg->is_error)
2945         {
2946             /* no sense in comparing the data, something is wrong */
2947             r = 0; /* fail */
2948         }
2949         else
2950         {
2951             /* compare the data */
2952             x = memcmp(pReply, pPayload, length);
2953             if(x != 0)
2954             {
2955                 MT_MSG_log(LOG_ERROR,
2956                            pMsg->pSrsp,
2957                            "loop back data does not match\n");
2958                 r = 0; /* fail */
2959             }
2960         }
2961     }
2962     MT_MSG_free(pMsg);
2963     return (r);
2964 }
2965
2966 /*
2967  *  ========================================
2968  *  Texas Instruments Micro Controller Style
2969  *  ========================================
2970  *  Local Variables:
2971  *  mode: c
2972  *  c-file-style: "bsd"
2973  *  tab-width: 4
2974  *  c-basic-offset: 4
2975  *  indent-tabs-mode: nil
2976  *  End:
2977  *  vim:set  filetype=c tabstop=4 shiftwidth=4 expandtab=true
2978  */