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