git.zerfleddert.de Git - hmcfgusb/blob - flash-ota.c

   1 /* flasher for HomeMatic-devices supporting OTA updates
   2  *
   3  * Copyright (c) 2014 Michael Gernoth <michael@gernoth.net>
   4  *
   5  * Permission is hereby granted, free of charge, to any person obtaining a copy
   6  * of this software and associated documentation files (the "Software"), to
   7  * deal in the Software without restriction, including without limitation the
   8  * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
   9  * sell copies of the Software, and to permit persons to whom the Software is
  10  * furnished to do so, subject to the following conditions:
  11  *
  12  * The above copyright notice and this permission notice shall be included in
  13  * all copies or substantial portions of the Software.
  14  *
  15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  16  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  17  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  18  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  19  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
  20  * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
  21  * IN THE SOFTWARE.
  22  */
  23
  24 #include <stdio.h>
  25 #include <stdlib.h>
  26 #include <unistd.h>
  27 #include <stdint.h>
  28 #include <string.h>
  29 #include <strings.h>
  30 #include <poll.h>
  31 #include <errno.h>
  32 #include <sys/types.h>
  33 #include <sys/stat.h>
  34 #include <fcntl.h>
  35 #include <sys/time.h>
  36 #include <libusb-1.0/libusb.h>
  37
  38 #include "hexdump.h"
  39 #include "firmware.h"
  40 #include "hm.h"
  41 #include "version.h"
  42 #include "hmcfgusb.h"
  43 #include "culfw.h"
  44 #include "util.h"
  45
  46 #define MAX_RETRIES     5
  47
  48 extern char *optarg;
  49
  50 uint32_t hmid = 0;
  51 uint32_t my_hmid = 0;
  52
  53 enum device_type {
  54         DEVICE_TYPE_HMCFGUSB,
  55         DEVICE_TYPE_CULFW,
  56 };
  57
  58 struct ota_dev {
  59         int type;
  60         struct hmcfgusb_dev *hmcfgusb;
  61         struct culfw_dev *culfw;
  62 };
  63
  64 enum message_type {
  65         MESSAGE_TYPE_E = 1,
  66         MESSAGE_TYPE_R = 2,
  67 };
  68
  69 struct recv_data {
  70         uint8_t message[64];
  71         enum message_type message_type;
  72         uint16_t status;
  73         int speed;
  74         uint16_t version;
  75 };
  76
  77 static int parse_hmcfgusb(uint8_t *buf, int buf_len, void *data)
  78 {
  79         struct recv_data *rdata = data;
  80
  81         if (buf_len < 1)
  82                 return 1;
  83
  84         switch (buf[0]) {
  85                 case 'E':
  86                         if ((!hmid) ||
  87                             ((buf[0x11] == ((hmid >> 16) & 0xff)) &&
  88                             (buf[0x12] == ((hmid >> 8) & 0xff)) &&
  89                             (buf[0x13] == (hmid & 0xff)))) {
  90                                 memset(rdata->message, 0, sizeof(rdata->message));
  91                                 memcpy(rdata->message, buf + 0x0d, buf[0x0d] + 1);
  92                                 rdata->message_type = MESSAGE_TYPE_E;
  93                         }
  94                         break;
  95                 case 'R':
  96                         memset(rdata->message, 0, sizeof(rdata->message));
  97                         memcpy(rdata->message, buf + 0x0e, buf[0x0e] + 1);
  98                         rdata->status = (buf[5] << 8) | buf[6];
  99                         rdata->message_type = MESSAGE_TYPE_R;
 100                         break;
 101                 case 'G':
 102                         rdata->speed = buf[1];
 103                         break;
 104                 case 'H':
 105                         rdata->version = (buf[11] << 8) | buf[12];
 106                         my_hmid = (buf[0x1b] << 16) | (buf[0x1c] << 8) | buf[0x1d];
 107                         break;
 108                 default:
 109                         break;
 110         }
 111
 112         if (buf_len != 1)
 113                 return 1;
 114
 115         return 1;
 116 }
 117
 118 static int parse_culfw(uint8_t *buf, int buf_len, void *data)
 119 {
 120         struct recv_data *rdata = data;
 121         int pos = 0;
 122
 123         memset(rdata, 0, sizeof(struct recv_data));
 124
 125         if (buf_len <= 3)
 126                 return 0;
 127
 128         switch(buf[0]) {
 129                 case 'A':
 130                         if (buf[1] == 's')
 131                                 return 0;
 132
 133                         while(validate_nibble(buf[(pos * 2) + 1]) &&
 134                               validate_nibble(buf[(pos * 2) + 2]) &&
 135                               (pos + 1 < buf_len)) {
 136                                 rdata->message[pos] = ascii_to_nibble(buf[(pos * 2) + 1]) << 4;
 137                                 rdata->message[pos] |= ascii_to_nibble(buf[(pos * 2) + 2]);
 138                                 pos++;
 139                         }
 140
 141                         if (hmid && (SRC(rdata->message) != hmid))
 142                                 return 0;
 143
 144                         rdata->message_type = MESSAGE_TYPE_E;
 145                         break;
 146                 case 'V':
 147                         {
 148                                 uint8_t v;
 149                                 char *s;
 150                                 char *e;
 151
 152                                 s = ((char*)buf) + 2;
 153                                 e = strchr(s, '.');
 154                                 if (!e) {
 155                                         fprintf(stderr, "Unknown response from CUL: %s", buf);
 156                                         return 0;
 157                                 }
 158                                 *e = '\0';
 159                                 v = atoi(s);
 160                                 rdata->version = v << 8;
 161
 162                                 s = e + 1;
 163                                 e = strchr(s, ' ');
 164                                 if (!e) {
 165                                         fprintf(stderr, "Unknown response from CUL: %s", buf);
 166                                         return 0;
 167                                 }
 168                                 *e = '\0';
 169                                 v = atoi(s);
 170                                 rdata->version |= v;
 171                         }
 172                         break;
 173                 default:
 174                         fprintf(stderr, "Unknown response from CUL: %s", buf);
 175                         return 0;
 176                         break;
 177         }
 178
 179         return 1;
 180 }
 181
 182 int send_hm_message(struct ota_dev *dev, struct recv_data *rdata, uint8_t *msg)
 183 {
 184         static uint32_t id = 1;
 185         struct timeval tv;
 186         uint8_t out[0x40];
 187         int pfd;
 188
 189         switch(dev->type) {
 190                 case DEVICE_TYPE_HMCFGUSB:
 191                         if (gettimeofday(&tv, NULL) == -1) {
 192                                 perror("gettimeofay");
 193                                         return 0;
 194                         }
 195
 196                         memset(out, 0, sizeof(out));
 197
 198                         out[0] = 'S';
 199                         out[1] = (id >> 24) & 0xff;
 200                         out[2] = (id >> 16) & 0xff;
 201                         out[3] = (id >> 8) & 0xff;
 202                         out[4] = id & 0xff;
 203                         out[10] = 0x01;
 204                         out[11] = (tv.tv_usec >> 24) & 0xff;
 205                         out[12] = (tv.tv_usec >> 16) & 0xff;
 206                         out[13] = (tv.tv_usec >> 8) & 0xff;
 207                         out[14] = tv.tv_usec & 0xff;
 208
 209                         memcpy(&out[0x0f], msg, msg[0] + 1);
 210
 211                         memset(rdata, 0, sizeof(struct recv_data));
 212                         hmcfgusb_send(dev->hmcfgusb, out, sizeof(out), 1);
 213
 214                         while (1) {
 215                                 if (rdata->message_type == MESSAGE_TYPE_R) {
 216                                         if (((rdata->status & 0xff) == 0x01) ||
 217                                             ((rdata->status & 0xff) == 0x02)) {
 218                                                 break;
 219                                         } else {
 220                                                 if ((rdata->status & 0xff00) == 0x0400) {
 221                                                         fprintf(stderr, "\nOut of credits!\n");
 222                                                 } else if ((rdata->status & 0xff) == 0x08) {
 223                                                         fprintf(stderr, "\nMissing ACK!\n");
 224                                                 } else {
 225                                                         fprintf(stderr, "\nInvalid status: %04x\n", rdata->status);
 226                                                 }
 227                                                 return 0;
 228                                         }
 229                                 }
 230                                 errno = 0;
 231                                 pfd = hmcfgusb_poll(dev->hmcfgusb, 1000);
 232                                 if ((pfd < 0) && errno) {
 233                                         if (errno != ETIMEDOUT) {
 234                                                 perror("\n\nhmcfgusb_poll");
 235                                                 exit(EXIT_FAILURE);
 236                                         }
 237                                 }
 238                         }
 239                         break;
 240                 case DEVICE_TYPE_CULFW:
 241                         {
 242                                 char buf[256];
 243                                 int i;
 244
 245                                 memset(buf, 0, sizeof(buf));
 246                                 buf[0] = 'A';
 247                                 buf[1] = 's';
 248                                 for (i = 0; i < msg[0] + 1; i++) {
 249                                         buf[2 + (i * 2)] = nibble_to_ascii((msg[i] >> 4) & 0xf);
 250                                         buf[2 + (i * 2) + 1] = nibble_to_ascii(msg[i] & 0xf);
 251                                 }
 252                                 buf[2 + (i * 2) ] = '\r';
 253                                 buf[2 + (i * 2) + 1] = '\n';
 254
 255                                 memset(rdata, 0, sizeof(struct recv_data));
 256                                 if (culfw_send(dev->culfw, buf, 2 + (i * 2) + 1) == 0) {
 257                                         fprintf(stderr, "culfw_send failed!\n");
 258                                         exit(EXIT_FAILURE);
 259                                 }
 260
 261                                 if (msg[CTL] & 0x20) {
 262                                         int cnt = 3;
 263                                         int pfd;
 264                                         do {
 265                                                 errno = 0;
 266                                                 pfd = culfw_poll(dev->culfw, 200);
 267                                                 if ((pfd < 0) && errno) {
 268                                                         if (errno != ETIMEDOUT) {
 269                                                                 perror("\n\nculfw_poll");
 270                                                                 exit(EXIT_FAILURE);
 271                                                         }
 272                                                 }
 273                                                 if (rdata->message_type == MESSAGE_TYPE_E) {
 274                                                         break;
 275                                                 }
 276                                         } while(cnt--);
 277
 278                                         if (cnt == -1) {
 279                                                 fprintf(stderr, "\nMissing ACK!\n");
 280                                                 return 0;
 281                                         }
 282                                 }
 283                         }
 284                         break;
 285         }
 286
 287         id++;
 288         return 1;
 289 }
 290
 291 static int switch_speed(struct ota_dev *dev, struct recv_data *rdata, uint8_t speed)
 292 {
 293         uint8_t out[0x40];
 294         int pfd;
 295
 296         printf("Entering %uk-mode\n", speed);
 297
 298         switch(dev->type) {
 299                 case DEVICE_TYPE_HMCFGUSB:
 300                         memset(out, 0, sizeof(out));
 301                         out[0] = 'G';
 302                         out[1] = speed;
 303
 304                         hmcfgusb_send(dev->hmcfgusb, out, sizeof(out), 1);
 305
 306                         while (1) {
 307                                 errno = 0;
 308                                 pfd = hmcfgusb_poll(dev->hmcfgusb, 1000);
 309                                 if ((pfd < 0) && errno) {
 310                                         if (errno != ETIMEDOUT) {
 311                                                 perror("\n\nhmcfgusb_poll");
 312                                                 exit(EXIT_FAILURE);
 313                                         }
 314                                 }
 315                                 if (rdata->speed == speed)
 316                                         break;
 317                         }
 318                         break;
 319                 case DEVICE_TYPE_CULFW:
 320                         if (speed == 100) {
 321                                 return culfw_send(dev->culfw, "AR\r\n", 4);
 322                         } else {
 323                                 return culfw_send(dev->culfw, "Ar\r\n", 4);
 324                         }
 325                         break;
 326         }
 327
 328         return 1;
 329 }
 330
 331 void flash_ota_syntax(char *prog)
 332 {
 333         fprintf(stderr, "Syntax: %s parameters options\n\n", prog);
 334         fprintf(stderr, "Mandatory parameters:\n");
 335         fprintf(stderr, "\t-f firmware.eq3\tfirmware file to flash\n");
 336         fprintf(stderr, "\t-s SERIAL\tserial of device to flash\n");
 337         fprintf(stderr, "\nPossible options:\n");
 338         fprintf(stderr, "\t-c device\tenable CUL-mode with CUL at path \"device\"\n");
 339         fprintf(stderr, "\t-b bps\t\tuse CUL with speed \"bps\" (default: %u)\n", DEFAULT_CUL_BPS);
 340         fprintf(stderr, "\t-h\t\tthis help\n");
 341 }
 342
 343 int main(int argc, char **argv)
 344 {
 345         const char twiddlie[] = { '-', '\\', '|', '/' };
 346         const uint8_t cc1101_regs[] = { 0x10, 0x5B, 0x11, 0xF8, 0x15, 0x47 };
 347         char *fw_file = NULL;
 348         char *serial = NULL;
 349         char *culfw_dev = NULL;
 350         unsigned int bps = DEFAULT_CUL_BPS;
 351         struct ota_dev dev;
 352         struct recv_data rdata;
 353         uint8_t out[0x40];
 354         uint8_t *pos;
 355         uint8_t msgid = 0x1;
 356         uint16_t len;
 357         struct firmware *fw;
 358         int block;
 359         int pfd;
 360         int debug = 0;
 361         int cnt;
 362         int switchcnt = 0;
 363         int msgnum = 0;
 364         int switched = 0;
 365         int opt;
 366
 367         printf("HomeMatic OTA flasher version " VERSION "\n\n");
 368
 369         while((opt = getopt(argc, argv, "b:c:f:hs:")) != -1) {
 370                 switch (opt) {
 371                         case 'b':
 372                                 bps = atoi(optarg);
 373                                 break;
 374                         case 'c':
 375                                 culfw_dev = optarg;
 376                                 break;
 377                         case 'f':
 378                                 fw_file = optarg;
 379                                 break;
 380                         case 's':
 381                                 serial = optarg;
 382                                 break;
 383                         case 'h':
 384                         case ':':
 385                         case '?':
 386                         default:
 387                                 flash_ota_syntax(argv[0]);
 388                                 exit(EXIT_FAILURE);
 389                                 break;
 390
 391                 }
 392         }
 393
 394         if (!fw_file || !serial) {
 395                 flash_ota_syntax(argv[0]);
 396                 exit(EXIT_FAILURE);
 397         }
 398
 399         fw = firmware_read_firmware(fw_file, debug);
 400         if (!fw)
 401                 exit(EXIT_FAILURE);
 402
 403         memset(&rdata, 0, sizeof(rdata));
 404         memset(&dev, 0, sizeof(struct ota_dev));
 405
 406         if (culfw_dev) {
 407                 printf("Opening culfw-device at path %s with speed %u\n", culfw_dev, bps);
 408                 dev.culfw = culfw_init(culfw_dev, bps, parse_culfw, &rdata);
 409                 if (!dev.culfw) {
 410                         fprintf(stderr, "Can't initialize CUL at %s with rate %u\n", culfw_dev, bps);
 411                         exit(EXIT_FAILURE);
 412                 }
 413                 dev.type = DEVICE_TYPE_CULFW;
 414
 415                 printf("Requesting firmware-version\n");
 416                 culfw_send(dev.culfw, "\r\n", 2);
 417                 culfw_flush(dev.culfw);
 418
 419                 while (1) {
 420                         culfw_send(dev.culfw, "V\r\n", 3);
 421
 422                         errno = 0;
 423                         pfd = culfw_poll(dev.culfw, 1000);
 424                         if ((pfd < 0) && errno) {
 425                                 if (errno != ETIMEDOUT) {
 426                                         perror("\n\nhmcfgusb_poll");
 427                                         exit(EXIT_FAILURE);
 428                                 }
 429                         }
 430                         if (rdata.version)
 431                                 break;
 432                 }
 433
 434                 printf("culfw-device firmware version: %u.%02u\n",
 435                         (rdata.version >> 8) & 0xff,
 436                         rdata.version & 0xff);
 437
 438                 if (rdata.version < 0x013a) {
 439                         fprintf(stderr, "\nThis version does _not_ support firmware upgrade mode, you need at least 1.58!\n");
 440                         exit(EXIT_FAILURE);
 441                 }
 442         } else {
 443                 hmcfgusb_set_debug(debug);
 444
 445                 dev.hmcfgusb = hmcfgusb_init(parse_hmcfgusb, &rdata);
 446                 if (!dev.hmcfgusb) {
 447                         fprintf(stderr, "Can't initialize HM-CFG-USB\n");
 448                         exit(EXIT_FAILURE);
 449                 }
 450                 dev.type = DEVICE_TYPE_HMCFGUSB;
 451
 452                 printf("\nRebooting HM-CFG-USB to avoid running out of credits\n\n");
 453
 454                 if (!dev.hmcfgusb->bootloader) {
 455                         printf("HM-CFG-USB not in bootloader mode, entering bootloader.\n");
 456                         printf("Waiting for device to reappear...\n");
 457
 458                         do {
 459                                 if (dev.hmcfgusb) {
 460                                         if (!dev.hmcfgusb->bootloader)
 461                                                 hmcfgusb_enter_bootloader(dev.hmcfgusb);
 462                                         hmcfgusb_close(dev.hmcfgusb);
 463                                 }
 464                                 sleep(1);
 465                         } while (((dev.hmcfgusb = hmcfgusb_init(parse_hmcfgusb, &rdata)) == NULL) || (!dev.hmcfgusb->bootloader));
 466                 }
 467
 468                 if (dev.hmcfgusb->bootloader) {
 469                         printf("HM-CFG-USB in bootloader mode, rebooting\n");
 470
 471                         do {
 472                                 if (dev.hmcfgusb) {
 473                                         if (dev.hmcfgusb->bootloader)
 474                                                 hmcfgusb_leave_bootloader(dev.hmcfgusb);
 475                                         hmcfgusb_close(dev.hmcfgusb);
 476                                 }
 477                                 sleep(1);
 478                         } while (((dev.hmcfgusb = hmcfgusb_init(parse_hmcfgusb, &rdata)) == NULL) || (dev.hmcfgusb->bootloader));
 479                 }
 480
 481                 printf("\n\nHM-CFG-USB opened\n\n");
 482
 483                 memset(out, 0, sizeof(out));
 484                 out[0] = 'K';
 485                 hmcfgusb_send(dev.hmcfgusb, out, sizeof(out), 1);
 486
 487                 while (1) {
 488                         errno = 0;
 489                         pfd = hmcfgusb_poll(dev.hmcfgusb, 1000);
 490                         if ((pfd < 0) && errno) {
 491                                 if (errno != ETIMEDOUT) {
 492                                         perror("\n\nhmcfgusb_poll");
 493                                         exit(EXIT_FAILURE);
 494                                 }
 495                         }
 496                         if (rdata.version)
 497                                 break;
 498                 }
 499
 500                 if (rdata.version < 0x3c7) {
 501                         fprintf(stderr, "HM-CFG-USB firmware too low: %u < 967\n", rdata.version);
 502                         exit(EXIT_FAILURE);
 503                 }
 504
 505                 printf("HM-CFG-USB firmware version: %u\n", rdata.version);
 506         }
 507
 508         if (!switch_speed(&dev, &rdata, 10)) {
 509                 fprintf(stderr, "Can't switch speed!\n");
 510                 exit(EXIT_FAILURE);
 511         }
 512
 513         printf("Waiting for device with serial %s\n", serial);
 514
 515         while (1) {
 516                 errno = 0;
 517                 switch (dev.type) {
 518                         case DEVICE_TYPE_CULFW:
 519                                 pfd = culfw_poll(dev.culfw, 1000);
 520                                 break;
 521                         case DEVICE_TYPE_HMCFGUSB:
 522                         default:
 523                                 pfd = hmcfgusb_poll(dev.hmcfgusb, 1000);
 524                                 break;
 525                 }
 526
 527                 if ((pfd < 0) && errno) {
 528                         if (errno != ETIMEDOUT) {
 529                                 perror("\n\npoll");
 530                                 exit(EXIT_FAILURE);
 531                         }
 532                 }
 533
 534                 if ((rdata.message[LEN] == 0x14) && /* Length */
 535                     (rdata.message[MSGID] == 0x00) && /* Message ID */
 536                     (rdata.message[CTL] == 0x00) && /* Control Byte */
 537                     (rdata.message[TYPE] == 0x10) && /* Messagte type: Information */
 538                     (DST(rdata.message) == 0x000000) && /* Broadcast */
 539                     (rdata.message[PAYLOAD] == 0x00)) { /* FUP? */
 540                         if (!strncmp((char*)&(rdata.message[0x0b]), serial, 10)) {
 541                                 hmid = SRC(rdata.message);
 542                                 break;
 543                         }
 544                 }
 545         }
 546
 547         printf("Device with serial %s (hmid: %06x) entered firmware-update-mode\n", serial, hmid);
 548
 549         if (dev.type == DEVICE_TYPE_HMCFGUSB) {
 550                 printf("Adding HMID\n");
 551
 552                 memset(out, 0, sizeof(out));
 553                 out[0] = '+';
 554                 out[1] = (hmid >> 16) & 0xff;
 555                 out[2] = (hmid >> 8) & 0xff;
 556                 out[3] = hmid & 0xff;
 557
 558                 hmcfgusb_send(dev.hmcfgusb, out, sizeof(out), 1);
 559         }
 560
 561         switchcnt = 3;
 562         do {
 563                 printf("Initiating remote switch to 100k\n");
 564
 565                 memset(out, 0, sizeof(out));
 566
 567                 out[MSGID] = msgid++;
 568                 out[CTL] = 0x00;
 569                 out[TYPE] = 0xCB;
 570                 SET_SRC(out, my_hmid);
 571                 SET_DST(out, hmid);
 572
 573                 memcpy(&out[PAYLOAD], cc1101_regs, sizeof(cc1101_regs));
 574                 SET_LEN_FROM_PAYLOADLEN(out, sizeof(cc1101_regs));
 575
 576                 if (!send_hm_message(&dev, &rdata, out)) {
 577                         exit(EXIT_FAILURE);
 578                 }
 579
 580                 if (!switch_speed(&dev, &rdata, 100)) {
 581                         fprintf(stderr, "Can't switch speed!\n");
 582                         exit(EXIT_FAILURE);
 583                 }
 584
 585                 printf("Has the device switched?\n");
 586
 587                 memset(out, 0, sizeof(out));
 588
 589                 out[MSGID] = msgid++;
 590                 out[CTL] = 0x20;
 591                 out[TYPE] = 0xCB;
 592                 SET_SRC(out, my_hmid);
 593                 SET_DST(out, hmid);
 594
 595                 memcpy(&out[PAYLOAD], cc1101_regs, sizeof(cc1101_regs));
 596                 SET_LEN_FROM_PAYLOADLEN(out, sizeof(cc1101_regs));
 597
 598                 cnt = 3;
 599                 do {
 600                         if (send_hm_message(&dev, &rdata, out)) {
 601                                 /* A0A02000221B9AD00000000 */
 602                                 switched = 1;
 603                                 break;
 604                         }
 605                 } while (cnt--);
 606
 607                 if (!switched) {
 608                         printf("No!\n");
 609
 610                         if (!switch_speed(&dev, &rdata, 10)) {
 611                                 fprintf(stderr, "Can't switch speed!\n");
 612                                 exit(EXIT_FAILURE);
 613                         }
 614                 }
 615         } while ((!switched) && (switchcnt--));
 616
 617         if (!switched) {
 618                 fprintf(stderr, "Too many errors, giving up!\n");
 619                 exit(EXIT_FAILURE);
 620         }
 621
 622         printf("Yes!\n");
 623
 624         printf("Flashing %d blocks", fw->fw_blocks);
 625         if (debug) {
 626                 printf("\n");
 627         } else {
 628                 printf(": %04u/%04u %c", 0, fw->fw_blocks, twiddlie[0]);
 629                 fflush(stdout);
 630         }
 631
 632         for (block = 0; block < fw->fw_blocks; block++) {
 633                 int first;
 634
 635                 len = fw->fw[block][2] << 8;
 636                 len |= fw->fw[block][3];
 637
 638                 pos = &(fw->fw[block][2]);
 639
 640                 len += 2; /* length */
 641
 642                 if (debug)
 643                         hexdump(pos, len, "F> ");
 644
 645                 first = 1;
 646                 cnt = 0;
 647                 do {
 648                         int payloadlen = 35;
 649                         int ack = 0;
 650
 651                         if (first) {
 652                                 payloadlen = 37;
 653                                 first = 0;
 654                         }
 655
 656                         if ((len - (pos - &(fw->fw[block][2]))) < payloadlen)
 657                                 payloadlen = (len - (pos - &(fw->fw[block][2])));
 658
 659                         if (((pos + payloadlen) - &(fw->fw[block][2])) == len)
 660                                 ack = 1;
 661
 662                         memset(&rdata, 0, sizeof(rdata));
 663
 664                         memset(out, 0, sizeof(out));
 665
 666                         out[MSGID] = msgid;
 667                         if (ack)
 668                                 out[CTL] = 0x20;
 669                         out[TYPE] = 0xCA;
 670                         SET_SRC(out, my_hmid);
 671                         SET_DST(out, hmid);
 672
 673                         memcpy(&out[PAYLOAD], pos, payloadlen);
 674                         SET_LEN_FROM_PAYLOADLEN(out, payloadlen);
 675
 676                         if (send_hm_message(&dev, &rdata, out)) {
 677                                 pos += payloadlen;
 678                         } else {
 679                                 pos = &(fw->fw[block][2]);
 680                                 cnt++;
 681                                 if (cnt == MAX_RETRIES) {
 682                                         fprintf(stderr, "\nToo many errors, giving up!\n");
 683                                         exit(EXIT_FAILURE);
 684                                 } else {
 685                                         printf("Flashing %d blocks: %04u/%04u %c", fw->fw_blocks, block + 1, fw->fw_blocks, twiddlie[msgnum % sizeof(twiddlie)]);
 686                                 }
 687                         }
 688
 689                         msgnum++;
 690
 691                         if (!debug) {
 692                                 printf("\b\b\b\b\b\b\b\b\b\b\b%04u/%04u %c",
 693                                         block + 1, fw->fw_blocks, twiddlie[msgnum % sizeof(twiddlie)]);
 694                                 fflush(stdout);
 695                         }
 696                 } while((pos - &(fw->fw[block][2])) < len);
 697                 msgid++;
 698         }
 699
 700         firmware_free(fw);
 701
 702         printf("\n");
 703
 704         if (!switch_speed(&dev, &rdata, 10)) {
 705                 fprintf(stderr, "Can't switch speed!\n");
 706                 exit(EXIT_FAILURE);
 707         }
 708
 709         printf("Waiting for device to reboot\n");
 710
 711         cnt = 10;
 712         do {
 713                 errno = 0;
 714                 switch(dev.type) {
 715                         case DEVICE_TYPE_CULFW:
 716                                 pfd = culfw_poll(dev.culfw, 1000);
 717                                 break;
 718                         case DEVICE_TYPE_HMCFGUSB:
 719                         default:
 720                                 pfd = hmcfgusb_poll(dev.hmcfgusb, 1000);
 721                                 break;
 722                 }
 723                 if ((pfd < 0) && errno) {
 724                         if (errno != ETIMEDOUT) {
 725                                 perror("\n\npoll");
 726                                 exit(EXIT_FAILURE);
 727                         }
 728                 }
 729                 if (rdata.message_type == MESSAGE_TYPE_E) {
 730                         break;
 731                 }
 732         } while(cnt--);
 733
 734         if (rdata.message_type == MESSAGE_TYPE_E) {
 735                 printf("Device rebooted\n");
 736         }
 737
 738         switch(dev.type) {
 739                 case DEVICE_TYPE_HMCFGUSB:
 740                         hmcfgusb_close(dev.hmcfgusb);
 741                         hmcfgusb_exit();
 742                         break;
 743                 case DEVICE_TYPE_CULFW:
 744                         culfw_close(dev.culfw);
 745                         break;
 746         }
 747
 748         return EXIT_SUCCESS;
 749 }