rom/usb/shellapps/PencamTool.c

   1 /*
   2  *----------------------------------------------------------------------------
   3  *                         Pencam Tool for Poseidon
   4  *----------------------------------------------------------------------------
   5  *                   By Chris Hodges <chrisly@platon42.de>
   6  */
   7
   8 #include "debug.h"
   9
  10 #include <proto/dos.h>
  11 #include <proto/exec.h>
  12 #include <proto/poseidon.h>
  13 #include <proto/intuition.h>
  14 #include <proto/graphics.h>
  15 #include <proto/diskfont.h>
  16
  17 #include "PencamTool.h"
  18 #include <math.h>
  19 #include <stdlib.h>
  20 #include <string.h>
  21
  22 #define ARGS_TO       0
  23 #define ARGS_PICNUM   1
  24 #define ARGS_INTERVAL 2
  25 #define ARGS_UPTO     3
  26 #define ARGS_NOBEEP   4
  27 #define ARGS_GAMMA    5
  28 #define ARGS_SHARPEN  6
  29 #define ARGS_TEXT     7
  30 #define ARGS_FONT     8
  31 #define ARGS_FONTSIZE 9
  32 #define ARGS_UNIT     10
  33 #define ARGS_SIZEOF   11
  34
  35 static const char *prgname = "PencamTool";
  36 static const char *template = "TO/A,PICNUM/N,INTERVAL/N,UPTO/N/K,NOBEEP/S,GAMMA/K,SHARPEN/S,TEXT/K,FONT/K,FONTSIZE/N/K,UNIT/N/K";
  37 static const char *version = "$VER: PencamTool 1.7 (12.06.09) by Chris Hodges <chrisly@platon42.de>";
  38 static IPTR ArgsArray[ARGS_SIZEOF];
  39 static struct RDArgs *ArgsHook = NULL;
  40
  41 static UWORD gammaredtab[256];
  42 static UWORD gammagreentab[256];
  43 static UWORD gammabluetab[256];
  44
  45 struct RastPort fontrp;
  46 struct RastPort picrp;
  47 struct TextAttr avenirta;
  48 struct TextFont *avenirfont = NULL;
  49 struct BitMap *fontbm = NULL;
  50 ULONG tlength, theight;
  51
  52 struct Library *ps;
  53
  54 AROS_UFP3(void, releasehook,
  55           AROS_UFPA(struct Hook *, hook, A0),
  56           AROS_UFPA(APTR, pab, A2),
  57           AROS_UFPA(struct NepClassPencam *, nch, A1));
  58
  59 struct NepClassPencam * SetupPencam(void);
  60 struct NepClassPencam * AllocPencam(struct NepClassPencam *nch);
  61 void FreePencam(struct NepClassPencam *nch);
  62
  63 #define GAMMA 0.450
  64 #define ZERO 17.0
  65
  66 void CreateGammaTab(void)
  67 {
  68     UWORD i;
  69     UWORD red, green, blue;
  70     double x,y;
  71     double gamma;
  72     if(!ArgsArray[ARGS_GAMMA])
  73     {
  74         return;
  75     }
  76     gamma = atof((char *) ArgsArray[ARGS_GAMMA]);
  77     gammaredtab[0] = gammagreentab[0] = gammabluetab[0] = 0;
  78     for(i=1; i<256; ++i)
  79     {
  80         x = i;
  81         x -= ZERO;
  82         if(x < 1.0)
  83         {
  84             x = 1.0;
  85         }
  86         y = pow((x/256.0), gamma)*255.0;
  87         red = (UWORD) (y*1.08);
  88         green = (UWORD) y;
  89         blue = (UWORD) (y*0.95);
  90         gammaredtab[i] = red < 256 ? red : 255;
  91         gammagreentab[i] = green;
  92         gammabluetab[i] = blue;
  93     }
  94 }
  95
  96 AROS_UFH3(void, releasehook,
  97           AROS_UFHA(struct Hook *, hook, A0),
  98           AROS_UFHA(APTR, pab, A2),
  99           AROS_UFHA(struct NepClassPencam *, nch, A1))
 100 {
 101     AROS_USERFUNC_INIT
 102     /*psdAddErrorMsg(RETURN_WARN, (STRPTR) prgname,
 103                    "Pencam killed!");*/
 104     Signal(nch->nch_Task, SIGBREAKF_CTRL_C);
 105     AROS_USERFUNC_EXIT
 106 }
 107
 108 struct NepClassPencam * SetupPencam(void)
 109 {
 110     struct NepClassPencam *nch;
 111     struct PsdDevice *pd = NULL;
 112     struct PsdAppBinding *pab;
 113     ULONG unit;
 114
 115     if(ArgsArray[ARGS_UNIT])
 116     {
 117         unit = *((ULONG *) ArgsArray[ARGS_UNIT]);
 118     } else {
 119         unit = 0;
 120     }
 121     do
 122     {
 123         do
 124         {
 125             pd = psdFindDevice(pd,
 126                                DA_VendorID, 0x0553,
 127                                DA_ProductID, 0x0202,
 128                                TAG_END);
 129         } while(pd && (unit--));
 130
 131         if(!pd)
 132         {
 133             PutStr("No Pencam found!\n");
 134             return(NULL);
 135         }
 136         if((nch = psdAllocVec(sizeof(struct NepClassPencam))))
 137         {
 138             nch->nch_Device = pd;
 139             nch->nch_ReleaseHook.h_Entry = (APTR) releasehook;
 140
 141             pab = psdClaimAppBinding(ABA_Device, pd,
 142                                      ABA_ReleaseHook, &nch->nch_ReleaseHook,
 143                                      ABA_UserData, nch,
 144                                      TAG_END);
 145             if(pab)
 146             {
 147                 if(AllocPencam(nch))
 148                 {
 149                     return(nch);
 150                 } else {
 151                     PutStr("Couldn't allocate Pencam...\n");
 152                 }
 153                 psdReleaseAppBinding(pab);
 154             } else {
 155                 PutStr("Couldn't claim binding!\n");
 156             }
 157             psdFreeVec(nch);
 158         }
 159         PutStr("Hohum...\n");
 160     } while(FALSE);
 161     return(NULL);
 162 }
 163
 164 struct RastPort * SetupText(void)
 165 {
 166     STRPTR text = (STRPTR) ArgsArray[ARGS_TEXT];
 167     InitRastPort(&fontrp);
 168     if(ArgsArray[ARGS_FONT])
 169     {
 170         avenirta.ta_Name = (STRPTR) ArgsArray[ARGS_FONT];
 171         if(ArgsArray[ARGS_FONTSIZE])
 172         {
 173             avenirta.ta_YSize = *((ULONG *) ArgsArray[ARGS_FONTSIZE]);
 174         } else {
 175             avenirta.ta_YSize = 8;
 176         }
 177         avenirta.ta_Style = 0L;
 178         avenirta.ta_Flags = 0L;
 179         if(!(avenirfont = OpenDiskFont(&avenirta)))
 180         {
 181             Printf("Couldn't open font!\n");
 182         } else {
 183             SetFont(&fontrp, avenirfont);
 184         }
 185     }
 186     tlength = TextLength(&fontrp, text, (ULONG) strlen(text));
 187     theight = fontrp.Font->tf_YSize;
 188     if(!(fontbm = AllocBitMap(tlength, theight, 1L, BMF_CLEAR, NULL)))
 189     {
 190         Printf("Couldn't allocate font bitmap memory (%ldx%ld)", tlength, theight);
 191         return(NULL);
 192     }
 193     fontrp.BitMap = fontbm;
 194     //printf("String: %s\nLength: %d\n",(char *)argsarray[ARGS_TEXT], tlength);
 195     Move(&fontrp, 0, (LONG) fontrp.Font->tf_Baseline);
 196     Text(&fontrp, text, (ULONG) strlen(text));
 197     return(&fontrp);
 198 }
 199
 200 void FreeText(void)
 201 {
 202     if(fontbm)
 203     {
 204         FreeBitMap(fontbm);
 205         fontbm = NULL;
 206     }
 207     if(avenirfont)
 208     {
 209         CloseFont(avenirfont);
 210         avenirfont = NULL;
 211     }
 212 }
 213
 214 void PasteText(struct PCImageHeader *pcih, UBYTE *output)
 215 {
 216     LONG x, y;
 217     LONG tarx, tary;
 218     LONG vw = pcih->pcih_ImgWidth;
 219     LONG vw3 = vw+vw+vw;
 220     UBYTE *op;
 221     WORD pix;
 222     if(tlength < vw-2)
 223     {
 224         tarx = (vw-tlength) >> 1;
 225     } else {
 226         tarx = 1;
 227     }
 228     tary = (pcih->pcih_ImgHeight-theight-theight);
 229     for(y = 0; y < theight; y++)
 230     {
 231         for(x = 0; (x < tlength) && (x+tarx+2 < vw); x++)
 232         {
 233             pix = ReadPixel(&fontrp, x, y);
 234             if(pix)
 235             {
 236                 op = &output[((tary+y)*vw+tarx+x)*3];
 237                 op[-vw3] >>= 1;
 238                 op[-vw3+1] >>= 1;
 239                 op[-vw3+2] >>= 1;
 240                 op[-3] >>= 1;
 241                 op[-2] >>= 1;
 242                 op[-1] >>= 1;
 243                 op[3] >>= 1;
 244                 op[4] >>= 1;
 245                 op[5] >>= 1;
 246                 op[vw3] >>= 1;
 247                 op[vw3+1] >>= 1;
 248                 op[vw3+2] >>= 1;
 249             }
 250         }
 251     }
 252     for(y = 0; y < theight; y++)
 253     {
 254         for(x = 0; (x < tlength) && (x+tarx+2 < vw); x++)
 255         {
 256             pix = ReadPixel(&fontrp, x, y);
 257             if(pix)
 258             {
 259                 op = &output[((tary+y)*vw+tarx+x)*3];
 260                 *op++ = 255;
 261                 *op++ = 255;
 262                 *op = 255;
 263             }
 264         }
 265     }
 266 }
 267
 268 struct NepClassPencam * AllocPencam(struct NepClassPencam *nch)
 269 {
 270     struct List *cfglist;
 271     struct List *iflist;
 272     struct List *altiflist;
 273     IPTR ifnum;
 274     IPTR altnum;
 275     struct List *eplist;
 276
 277     nch->nch_Task = FindTask(NULL);
 278
 279     psdGetAttrs(PGA_DEVICE, nch->nch_Device,
 280                 DA_ConfigList, &cfglist,
 281                 TAG_END);
 282
 283     if(!cfglist->lh_Head->ln_Succ)
 284     {
 285         PutStr("No configs?\n");
 286         return(NULL);
 287     }
 288
 289     nch->nch_Config = (struct PsdConfig *) cfglist->lh_Head;
 290
 291     psdGetAttrs(PGA_CONFIG, nch->nch_Config,
 292                 CA_InterfaceList, &iflist,
 293                 TAG_END);
 294
 295     if(!iflist->lh_Head->ln_Succ)
 296     {
 297         PutStr("No interfaces?\n");
 298         return(NULL);
 299     }
 300
 301     nch->nch_Interface = (struct PsdInterface *) iflist->lh_Head;
 302     psdGetAttrs(PGA_INTERFACE, nch->nch_Interface,
 303                 IFA_InterfaceNum, &ifnum,
 304                 IFA_AlternateNum, &altnum,
 305                 IFA_AlternateIfList, &altiflist,
 306                 IFA_EndpointList, &eplist,
 307                 TAG_END);
 308
 309     if((nch->nch_TaskMsgPort = CreateMsgPort()))
 310     {
 311         if((nch->nch_EP0Pipe = psdAllocPipe(nch->nch_Device, nch->nch_TaskMsgPort, NULL)))
 312         {
 313             if((ifnum == 0) && (altnum == 0))
 314             {
 315                 psdSetAltInterface(nch->nch_EP0Pipe, altiflist->lh_Head);
 316             }
 317             psdGetAttrs(PGA_CONFIG, nch->nch_Config,
 318                         CA_InterfaceList, &iflist,
 319                         TAG_END);
 320             nch->nch_Interface = (struct PsdInterface *) iflist->lh_Head;
 321             psdGetAttrs(PGA_INTERFACE, nch->nch_Interface,
 322                         IFA_InterfaceNum, &ifnum,
 323                         IFA_AlternateNum, &altnum,
 324                         IFA_EndpointList, &eplist,
 325                         TAG_END);
 326             if(eplist->lh_Head->ln_Succ)
 327             {
 328                 nch->nch_BulkEP = (struct PsdEndpoint *) eplist->lh_Head;
 329                 psdGetAttrs(PGA_ENDPOINT, nch->nch_BulkEP,
 330                             EA_MaxPktSize, &nch->nch_BulkPktSize,
 331                             TAG_END);
 332                 if((nch->nch_BulkPipe = psdAllocPipe(nch->nch_Device, nch->nch_TaskMsgPort, nch->nch_BulkEP)))
 333                 {
 334                     psdSetAttrs(PGA_PIPE, nch->nch_BulkPipe,
 335                                 PPA_AllowRuntPackets, TRUE,
 336                                 PPA_NakTimeout, TRUE,
 337                                 PPA_NakTimeoutTime, 5000,
 338                                 TAG_END);
 339                     return(nch);
 340                 } else {
 341                     PutStr("Couldn't allocate bulk pipe.\n");
 342                 }
 343             } else {
 344                 PutStr("No bulk endpoint?\n");
 345             }
 346             psdFreePipe(nch->nch_EP0Pipe);
 347         } else {
 348              PutStr("Couldn't allocate default pipe\n");
 349         }
 350         DeleteMsgPort(nch->nch_TaskMsgPort);
 351     }
 352     return(NULL);
 353 }
 354
 355
 356 void FreePencam(struct NepClassPencam *nch)
 357 {
 358     APTR pab;
 359
 360     psdGetAttrs(PGA_DEVICE, nch->nch_Device,
 361                 DA_Binding, &pab,
 362                 TAG_END);
 363     psdReleaseAppBinding(pab);
 364     psdFreePipe(nch->nch_BulkPipe);
 365     psdFreePipe(nch->nch_EP0Pipe);
 366     DeleteMsgPort(nch->nch_TaskMsgPort);
 367     psdFreeVec(nch);
 368 }
 369
 370 /**************************************************************************/
 371
 372 void bayer_unshuffle(struct PCImageHeader *pcih, UBYTE *raw, UBYTE *output)
 373 {
 374     ULONG x, y;
 375     ULONG w = pcih->pcih_ImgWidth>>1;
 376     ULONG vw = pcih->pcih_ImgWidth;
 377     ULONG vh = pcih->pcih_ImgHeight;
 378     UBYTE *raweven;
 379     UBYTE *rawodd;
 380     UBYTE *oline;
 381
 382     for(y = 0; y < vh; y++)
 383     {
 384         rawodd = &raw[y*vw];
 385         raweven = &rawodd[w];
 386         oline = output;
 387         if(y & 1)
 388         {
 389             ++oline;
 390         }
 391         ++oline;
 392         x = w;
 393         do
 394         {
 395             *oline = *raweven++;
 396             oline += 2;
 397             *oline = *rawodd++;
 398             oline += 4;
 399         } while(--x);
 400         output += vw;
 401         output += vw;
 402         output += vw;
 403     }
 404 }
 405
 406 void bayer_demosaic(struct PCImageHeader *pcih, UBYTE *output)
 407 {
 408     LONG x, y;
 409     LONG vw = pcih->pcih_ImgWidth;
 410     LONG vw3 = vw+vw+vw;
 411     LONG vh = pcih->pcih_ImgHeight;
 412     UBYTE *op;
 413     for(y = 1; y < vh-1; y++)
 414     {
 415         op = &output[(y*vw + 1)*3];
 416         for(x = 1; x < vw-1; x++) /* work out pixel type */
 417         {
 418             switch(((y + y) & 2) + (x & 1))
 419             {
 420                 case 0:        /* green. red lr, blue tb */
 421                     *op = (((UWORD) op[-3]) + ((UWORD) op[3])) >> 1; /* Set red */
 422                     op[2] = (((UWORD) op[2-vw3]) + ((UWORD) op[2+vw3]) + 1) >> 1; /* Set blue */
 423                     break;
 424                 case 1:        /* red. green lrtb, blue diagonals */
 425                     op[1] = (((UWORD) op[-2]) + ((UWORD) op[4]) +
 426                              ((UWORD) op[1-vw3]) + ((UWORD) op[1+vw3]) + 2) >> 2; /* Set green */
 427                     op[2] = (((UWORD) op[-1-vw3]) + ((UWORD) op[5-vw3]) +
 428                              ((UWORD) op[-1+vw3]) + ((UWORD) op[5+vw3]) + 2) >> 2; /* Set blue */
 429                     break;
 430                 case 2:        /* blue. green lrtb, red diagonals */
 431                     op[1] = (((UWORD) op[-2]) + ((UWORD) op[4]) +
 432                              ((UWORD) op[1-vw3]) + ((UWORD) op[1+vw3]) + 2) >> 2; /* Set green */
 433                     *op = (((UWORD) op[-3-vw3]) + ((UWORD) op[3-vw3]) +
 434                            ((UWORD) op[-3+vw3]) + ((UWORD) op[3+vw3]) + 2) >> 2; /* Set red */
 435                     break;
 436                 case 3:        /* green. blue lr, red tb */
 437                     op[2] = (((UWORD) op[-1]) + ((UWORD) op[5]) + 1) >> 1; /* Set blue */
 438                     *op = (((UWORD) op[-vw3]) + ((UWORD) op[vw3]) + 1) >> 1; /* Set red */
 439                     break;
 440             }  /* switch */
 441             op += 3;
 442         }   /* for x */
 443     }  /* for y */
 444 }
 445
 446 void gammacorrection(struct PCImageHeader *pcih, UBYTE *output)
 447 {
 448     ULONG cnt = pcih->pcih_ImgWidth*pcih->pcih_ImgHeight;
 449     while(cnt--)
 450     {
 451         *output = gammaredtab[*output];
 452         output++;
 453         *output = gammagreentab[*output];
 454         output++;
 455         *output = gammabluetab[*output];
 456         output++;
 457     }
 458 }
 459
 460 void sharpen5x5(struct PCImageHeader *pcih, UBYTE *input, UBYTE *output)
 461 {
 462     LONG x, y;
 463     LONG vw = pcih->pcih_ImgWidth;
 464     LONG vw3 = vw+vw+vw;
 465     LONG vw6 = vw*6;
 466     LONG vh = pcih->pcih_ImgHeight;
 467     LONG linem2[3];
 468     LONG linem1[3];
 469     LONG linep1[3];
 470     LONG linep2[3];
 471     LONG val[3];
 472
 473     UBYTE *op;
 474     UBYTE *oop;
 475     for(y = 2; y < vh-2; y++)
 476     {
 477         op = &input[((y-2)*vw)*3];
 478         linem2[0] = *op++;  // -2
 479         linem2[1] = *op++;
 480         linem2[2] = *op++;
 481         linem2[0] += *op++; // -1
 482         linem2[1] += *op++;
 483         linem2[2] += *op++;
 484         linem2[0] += *op++; // 0
 485         linem2[1] += *op++;
 486         linem2[2] += *op++;
 487         linem2[0] += *op++; // 1
 488         linem2[1] += *op++;
 489         linem2[2] += *op++;
 490         linem2[0] += *op++; // 2
 491         linem2[1] += *op++;
 492         linem2[2] += *op;
 493
 494         op = &input[((y-1)*vw+1)*3];
 495         linem1[0] = *op++;  // -1
 496         linem1[1] = *op++;
 497         linem1[2] = *op++;
 498         linem1[0] += *op++; // 0
 499         linem1[1] += *op++;
 500         linem1[2] += *op++;
 501         linem1[0] += *op++; // 1
 502         linem1[1] += *op++;
 503         linem1[2] += *op++;
 504         linem1[0] += linem1[0]+linem1[0];
 505         linem1[1] += linem1[1]+linem1[1];
 506         linem1[2] += linem1[2]+linem1[2];
 507         linem1[0] += *op++; // 2
 508         linem1[1] += *op++;
 509         linem1[2] += *op++;
 510         linem1[0] += op[-15]; // -2
 511         linem1[1] += op[-14];
 512         linem1[2] += op[-13];
 513
 514         op = &input[((y+1)*vw+1)*3];
 515         linep1[0] = *op++;  // -1
 516         linep1[1] = *op++;
 517         linep1[2] = *op++;
 518         linep1[0] += *op++; // 0
 519         linep1[1] += *op++;
 520         linep1[2] += *op++;
 521         linep1[0] += *op++; // 1
 522         linep1[1] += *op++;
 523         linep1[2] += *op++;
 524         linep1[0] += linep1[0]+linep1[0];
 525         linep1[1] += linep1[1]+linep1[1];
 526         linep1[2] += linep1[2]+linep1[2];
 527         linep1[0] += *op++; // 2
 528         linep1[1] += *op++;
 529         linep1[2] += *op++;
 530         linep1[0] += op[-15]; // -2
 531         linep1[1] += op[-14];
 532         linep1[2] += op[-13];
 533
 534         op = &input[((y+2)*vw)*3];
 535         linep2[0] = *op++;  // -2
 536         linep2[1] = *op++;
 537         linep2[2] = *op++;
 538         linep2[0] += *op++; // -1
 539         linep2[1] += *op++;
 540         linep2[2] += *op++;
 541         linep2[0] += *op++; // 0
 542         linep2[1] += *op++;
 543         linep2[2] += *op++;
 544         linep2[0] += *op++; // 1
 545         linep2[1] += *op++;
 546         linep2[2] += *op++;
 547         linep2[0] += *op++; // 2
 548         linep2[1] += *op++;
 549         linep2[2] += *op;
 550
 551         op = &input[(y*vw + 2)*3];
 552         oop = &output[(y*vw + 2)*3];
 553         for(x = 2; x < vw-2; x++) /* work out pixel type */
 554         {
 555             /* Central line */
 556             val[0] = op[-3] + op[3];
 557             val[1] = op[-2] + op[4];
 558             val[2] = op[-1] + op[5];
 559             val[0] += val[0] + val[0] + op[-6] + op[6] + linem2[0] + linem1[0] + linep1[0] + linep2[0];
 560             val[1] += val[1] + val[1] + op[-5] + op[7] + linem2[1] + linem1[1] + linep1[1] + linep2[1];
 561             val[2] += val[2] + val[2] + op[-4] + op[8] + linem2[2] + linem1[2] + linep1[2] + linep2[2];
 562             val[0] -= op[0] * 56;
 563             val[1] -= op[1] * 56;
 564             val[2] -= op[2] * 56;
 565 #define MAXVAL 4080
 566
 567             *oop++ = (val[0] > 0) ? 0 : ((val[0] < -MAXVAL) ? 255 : (-val[0]+8)>>4);
 568             *oop++ = (val[1] > 0) ? 0 : ((val[1] < -MAXVAL) ? 255 : (-val[1]+8)>>4);
 569             *oop++ = (val[2] > 0) ? 0 : ((val[2] < -MAXVAL) ? 255 : (-val[2]+8)>>4);
 570
 571             /* Update line y-2 */
 572             linem2[0] -= op[-vw6-6];
 573             linem2[0] += op[-vw6+9];
 574             linem2[1] -= op[-vw6-5];
 575             linem2[1] += op[-vw6+10];
 576             linem2[2] -= op[-vw6-4];
 577             linem2[2] += op[-vw6+11];
 578
 579             /* Update line y-1 */
 580             linem1[0] -= op[-vw3-6];
 581             linem1[0] -= op[-vw3-3]<<1;
 582             linem1[0] += op[-vw3+6]<<1;
 583             linem1[0] += op[-vw3+9];
 584             linem1[1] -= op[-vw3-5];
 585             linem1[1] -= op[-vw3-2]<<1;
 586             linem1[1] += op[-vw3+7]<<1;
 587             linem1[1] += op[-vw3+10];
 588             linem1[2] -= op[-vw3-4];
 589             linem1[2] -= op[-vw3-1]<<1;
 590             linem1[2] += op[-vw3+8]<<1;
 591             linem1[2] += op[-vw3+11];
 592
 593             /* Update line y+1 */
 594             linep1[0] -= op[vw3-6];
 595             linep1[0] -= op[vw3-3]<<1;
 596             linep1[0] += op[vw3+6]<<1;
 597             linep1[0] += op[vw3+9];
 598             linep1[1] -= op[vw3-5];
 599             linep1[1] -= op[vw3-2]<<1;
 600             linep1[1] += op[vw3+7]<<1;
 601             linep1[1] += op[vw3+10];
 602             linep1[2] -= op[vw3-4];
 603             linep1[2] -= op[vw3-1]<<1;
 604             linep1[2] += op[vw3+8]<<1;
 605             linep1[2] += op[vw3+11];
 606
 607             /* Update line y-2 */
 608             linep2[0] -= op[vw6-6];
 609             linep2[0] += op[vw6+9];
 610             linep2[1] -= op[vw6-5];
 611             linep2[1] += op[vw6+10];
 612             linep2[2] -= op[vw6-4];
 613             linep2[2] += op[vw6+11];
 614
 615             op += 3;
 616         }
 617     }
 618 }
 619
 620 APTR TransferImage(struct NepClassPencam *nch, struct PCImageHeader *pcih)
 621 {
 622     LONG ioerr;
 623     UBYTE *rawbuf;
 624     UBYTE *imgbuf;
 625     UBYTE *newimgbuf;
 626
 627     rawbuf = psdAllocVec(pcih->pcih_ImgSize);
 628     if(!rawbuf)
 629     {
 630         Printf("Couldn't allocate %ld bytes of memory.\n", pcih->pcih_ImgSize);
 631         return(NULL);
 632     }
 633     /* Workaround for a firmware bug */
 634     ioerr = psdDoPipe(nch->nch_BulkPipe, rawbuf, 64);
 635     if(!ioerr)
 636     {
 637         if(((ULONG *) rawbuf)[0] == 0xed15ed15)
 638         {
 639             /* Junk packet at the beginning! */
 640             ioerr = psdDoPipe(nch->nch_BulkPipe, rawbuf, pcih->pcih_ImgSize);
 641         } else {
 642             ioerr = psdDoPipe(nch->nch_BulkPipe, &rawbuf[64], pcih->pcih_ImgSize-64);
 643         }
 644     }
 645     if(!ioerr)
 646     {
 647         imgbuf = psdAllocVec((ULONG) pcih->pcih_ImgWidth * (ULONG) pcih->pcih_ImgHeight * 3);
 648         if(imgbuf)
 649         {
 650             bayer_unshuffle(pcih, rawbuf, imgbuf);
 651             psdFreeVec(rawbuf);
 652             bayer_demosaic(pcih, imgbuf);
 653             if(ArgsArray[ARGS_SHARPEN])
 654             {
 655                 newimgbuf = psdAllocVec((ULONG) pcih->pcih_ImgWidth * (ULONG) pcih->pcih_ImgHeight * 3);
 656                 if(newimgbuf)
 657                 {
 658                     sharpen5x5(pcih, imgbuf, newimgbuf);
 659                     psdFreeVec(imgbuf);
 660                     imgbuf = newimgbuf;
 661                 }
 662             }
 663             if(ArgsArray[ARGS_GAMMA])
 664             {
 665                 gammacorrection(pcih, imgbuf);
 666             }
 667             if(ArgsArray[ARGS_TEXT])
 668             {
 669                 PasteText(pcih, imgbuf);
 670             }
 671             return(imgbuf);
 672         } else {
 673             Printf("Couldn't allocate %ld bytes of memory.\n", pcih->pcih_ImgSize);
 674         }
 675     } else {
 676         Printf("Bulk transfer failed: %s (%ld)\n",
 677                psdNumToStr(NTS_IOERR, ioerr, "unknown"), ioerr);
 678     }
 679     psdFreeVec(rawbuf);
 680     return(NULL);
 681 }
 682
 683 APTR GetPicture(struct NepClassPencam *nch, ULONG picnum, struct PCImageHeader *pcih)
 684 {
 685     struct PsdPipe *pp;
 686     LONG ioerr;
 687
 688     pp = nch->nch_EP0Pipe;
 689     psdPipeSetup(pp, URTF_IN|URTF_VENDOR|URTF_DEVICE,
 690                  CMDID_GET_IMAGE_HEADER, picnum, 0);
 691     ioerr = psdDoPipe(pp, pcih, sizeof(struct PCImageHeader));
 692     if(ioerr)
 693     {
 694         Printf("GET_IMAGE_HEADER failed: %s (%ld)\n",
 695                psdNumToStr(NTS_IOERR, ioerr, "unknown"), ioerr);
 696         return(NULL);
 697     }
 698
 699     psdPipeSetup(pp, URTF_OUT|URTF_VENDOR|URTF_DEVICE,
 700                  CMDID_UPLOAD_IMAGE, picnum, 0);
 701     ioerr = psdDoPipe(pp, pcih, sizeof(struct PCImageHeader));
 702     if(!ioerr)
 703     {
 704         return(TransferImage(nch, pcih));
 705     } else {
 706         Printf("UPLOAD_IMAGE failed: %s (%ld)\n",
 707                psdNumToStr(NTS_IOERR, ioerr, "unknown"), ioerr);
 708     }
 709     return(NULL);
 710 }
 711
 712 APTR GetVideoSnap(struct NepClassPencam *nch, struct PCImageHeader *pcih)
 713 {
 714     struct PsdPipe *pp;
 715     LONG ioerr;
 716
 717     pp = nch->nch_EP0Pipe;
 718     psdPipeSetup(pp, URTF_IN|URTF_VENDOR|URTF_DEVICE,
 719                  CMDID_GRAB_UPLOAD, ArgsArray[ARGS_NOBEEP] ? 0x6000L : 0x2000L, 0);
 720     ioerr = psdDoPipe(pp, pcih, 8);
 721     if(ioerr)
 722     {
 723         Printf("GRAB_UPLOAD failed: %s (%ld)\n",
 724                psdNumToStr(NTS_IOERR, ioerr, "unknown"), ioerr);
 725         return(NULL);
 726     }
 727     return(TransferImage(nch, pcih));
 728 }
 729
 730 int main(int argc, char *argv[])
 731 {
 732     struct NepClassPencam *nch;
 733     struct PCImageHeader pcih;
 734     BPTR outfile;
 735     UBYTE *imgbuf;
 736     ULONG sigs;
 737     char buf[256];
 738     ULONG imgcount;
 739     ULONG picnum;
 740
 741     if(!(ArgsHook = ReadArgs(template, ArgsArray, NULL)))
 742     {
 743         PutStr("Wrong arguments!\n");
 744         return(RETURN_FAIL);
 745     }
 746     ps = OpenLibrary("poseidon.library", 1);
 747     if(!ps)
 748     {
 749         FreeArgs(ArgsHook);
 750         return(RETURN_FAIL);
 751     }
 752     if(ArgsArray[ARGS_TEXT])
 753     {
 754         if(!(SetupText()))
 755         {
 756             FreeArgs(ArgsHook);
 757             CloseLibrary(ps);
 758             return(RETURN_ERROR);
 759         }
 760     }
 761     if(!(nch = SetupPencam()))
 762     {
 763         FreeText();
 764         FreeArgs(ArgsHook);
 765         CloseLibrary(ps);
 766         return(RETURN_ERROR);
 767     }
 768     CreateGammaTab();
 769     imgcount = 0;
 770     if(ArgsArray[ARGS_PICNUM])
 771     {
 772         picnum = *((ULONG *) ArgsArray[ARGS_PICNUM]);
 773     } else {
 774         picnum = 0;
 775     }
 776     do
 777     {
 778          imgbuf = NULL;
 779          if(ArgsArray[ARGS_PICNUM])
 780          {
 781              imgbuf = GetPicture(nch, picnum, &pcih);
 782          } else {
 783              imgbuf = GetVideoSnap(nch, &pcih);
 784          }
 785          if(imgbuf)
 786          {
 787              psdSafeRawDoFmt(buf, 256, (STRPTR) ArgsArray[ARGS_TO], imgcount);
 788              outfile = Open(buf, MODE_NEWFILE);
 789              if(outfile)
 790              {
 791                  UWORD y;
 792                  ULONG h = pcih.pcih_ImgHeight;
 793                  ULONG vh = h-4;
 794                  ULONG w = pcih.pcih_ImgWidth*3;
 795                  ULONG vw = w-4*3;
 796                  for(y = 0; y < vh; y++)
 797                  {
 798                      memcpy(imgbuf+y*vw, imgbuf+(y+2)*w+6, (size_t) vw);
 799                  }
 800                  FPrintf(outfile, "P6\n%ld %ld\n255\n", vw/3, vh);
 801                  Flush(outfile);
 802                  Write(outfile, imgbuf, vw*vh);
 803                  Close(outfile);
 804                  Printf("Wrote image into '%s'.\n", buf);
 805                  imgcount++;
 806              } else {
 807                  Printf("Could not open file '%s' for writing!\n", buf);
 808              }
 809              psdFreeVec(imgbuf);
 810          } else {
 811              break;
 812          }
 813          sigs = SetSignal(0, 0);
 814          if(sigs & SIGBREAKF_CTRL_C)
 815          {
 816              break;
 817          }
 818          if(ArgsArray[ARGS_INTERVAL])
 819          {
 820              if((!ArgsArray[ARGS_PICNUM]) && ArgsArray[ARGS_UPTO])
 821              {
 822                  if(imgcount > *((ULONG *) ArgsArray[ARGS_UPTO]))
 823                  {
 824                      break;
 825                  }
 826              }
 827              Delay(*((ULONG *) ArgsArray[ARGS_INTERVAL]));
 828          } else {
 829              if(ArgsArray[ARGS_UPTO])
 830              {
 831                  if(picnum < *((ULONG *) ArgsArray[ARGS_UPTO]))
 832                  {
 833                      picnum++;
 834                  } else {
 835                      break;
 836                  }
 837              } else {
 838                  break;
 839              }
 840          }
 841          sigs = SetSignal(0, 0);
 842          if(sigs & SIGBREAKF_CTRL_C)
 843          {
 844              break;
 845          }
 846     } while(TRUE);
 847     FreeText();
 848     FreePencam(nch);
 849     FreeArgs(ArgsHook);
 850     CloseLibrary(ps);
 851     return(RETURN_OK);
 852 }