rom/usb/shellapps/SonixcamTool.c

   1 /*
   2  *----------------------------------------------------------------------------
   3  *                         Sonixcam 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 "SonixcamTool.h"
  18 #include <math.h>
  19 #include <stdlib.h>
  20 #include <string.h>
  21
  22 #define ARGS_TO       0
  23 #define ARGS_INTERVAL 1
  24 #define ARGS_UPTO     2
  25 #define ARGS_GAMMA    3
  26 #define ARGS_SHARPEN  4
  27 #define ARGS_TEXT     5
  28 #define ARGS_FONT     6
  29 #define ARGS_FONTSIZE 7
  30 #define ARGS_UNIT     8
  31 #define ARGS_SIZEOF   9
  32
  33 const char SonixcamTool_prgname[] = "SonixcamTool";
  34 static const char *template = "TO/A,INTERVAL/N,UPTO/N/K,GAMMA/K,SHARPEN/S,TEXT/K,FONT/K,FONTSIZE/N/K,UNIT/N/K";
  35 const char SonixcamTool_version[] = "$VER: SonixcamTool 1.3 (12.06.09) by Chris Hodges <chrisly@platon42.de>";
  36 static IPTR ArgsArray[ARGS_SIZEOF];
  37 static struct RDArgs *ArgsHook = NULL;
  38
  39 static UWORD gammaredtab[256];
  40 static UWORD gammagreentab[256];
  41 static UWORD gammabluetab[256];
  42
  43 struct RastPort fontrp;
  44 struct RastPort picrp;
  45 struct TextAttr avenirta;
  46 struct TextFont *avenirfont = NULL;
  47 struct BitMap *fontbm = NULL;
  48 ULONG tlength, theight;
  49
  50 struct Library *ps;
  51
  52 AROS_UFP3(void, releasehook,
  53           AROS_UFPA(struct Hook *, hook, A0),
  54           AROS_UFPA(APTR, pab, A2),
  55           AROS_UFPA(struct NepClassSonixcam *, nch, A1));
  56
  57 AROS_UFP3(void, nInReqHook,
  58           AROS_UFPA(struct Hook *, hook, A0),
  59           AROS_UFPA(struct IOUsbHWRTIso *, urti, A2),
  60           AROS_UFPA(struct IOUsbHWBufferReq *, ubr, A1));
  61
  62 AROS_UFP3(void, nInDoneHook,
  63           AROS_UFPA(struct Hook *, hook, A0),
  64           AROS_UFPA(struct IOUsbHWRTIso *, urti, A2),
  65           AROS_UFPA(struct IOUsbHWBufferReq *, ubr, A1));
  66
  67 struct NepClassSonixcam * SetupSonixcam(void);
  68 struct NepClassSonixcam * AllocSonixcam(struct NepClassSonixcam *nch);
  69 void FreeSonixcam(struct NepClassSonixcam *nch);
  70
  71 #define GAMMA 0.450
  72 #define ZERO 17.0
  73
  74 void CreateGammaTab(void)
  75 {
  76     UWORD i;
  77     UWORD red, green, blue;
  78     double x,y;
  79     double gamma;
  80     if(!ArgsArray[ARGS_GAMMA])
  81     {
  82         return;
  83     }
  84     gamma = atof((char *) ArgsArray[ARGS_GAMMA]);
  85     gammaredtab[0] = gammagreentab[0] = gammabluetab[0] = 0;
  86     for(i=1; i<256; ++i)
  87     {
  88         x = i;
  89         x -= ZERO;
  90         if(x < 1.0)
  91         {
  92             x = 1.0;
  93         }
  94         y = pow((x/256.0), gamma)*255.0;
  95         red = (UWORD) (y*1.08);
  96         green = (UWORD) y;
  97         blue = (UWORD) (y*0.95);
  98         gammaredtab[i] = red < 256 ? red : 255;
  99         gammagreentab[i] = green;
 100         gammabluetab[i] = blue;
 101     }
 102 }
 103
 104 AROS_UFH3(void, releasehook,
 105           AROS_UFHA(struct Hook *, hook, A0),
 106           AROS_UFHA(APTR, pab, A2),
 107           AROS_UFHA(struct NepClassSonixcam *, nch, A1))
 108 {
 109     AROS_USERFUNC_INIT
 110     /*psdAddErrorMsg(RETURN_WARN, (STRPTR) prgname,
 111                    "Sonixcam killed!");*/
 112     Signal(nch->nch_Task, SIGBREAKF_CTRL_C);
 113     AROS_USERFUNC_EXIT
 114 }
 115
 116 AROS_UFH3(void, nInReqHook,
 117           AROS_UFHA(struct Hook *, hook, A0),
 118           AROS_UFHA(struct IOUsbHWRTIso *, urti, A2),
 119           AROS_UFHA(struct IOUsbHWBufferReq *, ubr, A1))
 120 {
 121     AROS_USERFUNC_INIT
 122
 123     struct NepClassSonixcam *nch = (struct NepClassSonixcam *) hook->h_Data;
 124
 125     ubr->ubr_Buffer = (UBYTE *) &nch->nch_CurrIsoBuf[nch->nch_IsoBufPos];
 126     if(ubr->ubr_Length + nch->nch_IsoBufPos > nch->nch_RawBufSize)
 127     {
 128         ubr->ubr_Length = 0;
 129     }
 130
 131     AROS_USERFUNC_EXIT
 132 }
 133
 134 AROS_UFH3(void, nInDoneHook,
 135           AROS_UFHA(struct Hook *, hook, A0),
 136           AROS_UFHA(struct IOUsbHWRTIso *, urti, A2),
 137           AROS_UFHA(struct IOUsbHWBufferReq *, ubr, A1))
 138 {
 139     AROS_USERFUNC_INIT
 140
 141     struct NepClassSonixcam *nch = (struct NepClassSonixcam *) hook->h_Data;
 142
 143     UBYTE *bufptr = &nch->nch_CurrIsoBuf[nch->nch_IsoBufPos];
 144     //KPRINTF(10, ("%ld\n", ubr->ubr_Length));
 145     nch->nch_FrameCnt++;
 146     if(!ubr->ubr_Length)
 147     {
 148         if(nch->nch_IsoBufPos > nch->nch_HeaderSize)
 149         {
 150             if(nch->nch_IsoBufPos == 320*240+nch->nch_HeaderSize)
 151             {
 152                 // image done
 153                 nch->nch_LastDoneNum = nch->nch_IsoBufNum;
 154                 nch->nch_BufState[nch->nch_IsoBufNum] = BUF_READY;
 155                 Signal(nch->nch_Task, 1UL<<nch->nch_ImgDoneSig);
 156                 if(++nch->nch_IsoBufNum == 3)
 157                 {
 158                     nch->nch_IsoBufNum = 0;
 159                 }
 160                 if(nch->nch_BufState[nch->nch_IsoBufNum] == BUF_BUSY)
 161                 {
 162                     if(++nch->nch_IsoBufNum == 3)
 163                     {
 164                         nch->nch_IsoBufNum = 0;
 165                     }
 166                 }
 167                 KPRINTF(10, ("Next Img: %ld\n", nch->nch_IsoBufNum));
 168                 nch->nch_CurrIsoBuf = nch->nch_RawBuf[nch->nch_IsoBufNum];
 169             } else {
 170                 KPRINTF(10, ("Restart at %ld/%ld\n", nch->nch_IsoBufPos, nch->nch_FrameCnt));
 171             }
 172             nch->nch_IsoBufPos = 0;
 173         }
 174         return;
 175     }
 176     if(nch->nch_IsoBufPos)
 177     {
 178         nch->nch_IsoBufPos += ubr->ubr_Length;
 179     } else {
 180         if((*((ULONG *) bufptr) == 0xffff00c4) && (*((UWORD *) &bufptr[4]) == 0xc496))
 181         {
 182             //KPRINTF(10, ("Started!\n"));
 183             nch->nch_IsoBufPos += ubr->ubr_Length;
 184             nch->nch_FrameCnt = 0;
 185         }
 186     }
 187
 188     AROS_USERFUNC_EXIT
 189 }
 190
 191 BOOL WriteReg(struct NepClassSonixcam *nch, ULONG index, ULONG value)
 192 {
 193     LONG ioerr;
 194     UBYTE buf = value;
 195
 196     //Printf("Writing register 0x%02lx with value 0x%02lx\n", index, value);
 197     psdPipeSetup(nch->nch_EP0Pipe, URTF_OUT|URTF_VENDOR|URTF_INTERFACE,
 198                  CMDID_WRITE_REG, index, nch->nch_IfNum);
 199     ioerr = psdDoPipe(nch->nch_EP0Pipe, &buf, 1);
 200     if(ioerr)
 201     {
 202         Printf("Error writing register 0x%02lx with value 0x%02lx, errorcode=%ld\n", index, value, ioerr);
 203         return FALSE;
 204     }
 205     nch->nch_Reg[index & 0xff] = value;
 206     return TRUE;
 207 }
 208
 209 BOOL WriteRegs(struct NepClassSonixcam *nch, ULONG index, ULONG len, UBYTE *buf)
 210 {
 211     LONG ioerr;
 212     ULONG cnt;
 213
 214     //Printf("Writing registers 0x%02lx with value 0x%02lx\n", index, *buf);
 215     psdPipeSetup(nch->nch_EP0Pipe, URTF_OUT|URTF_VENDOR|URTF_INTERFACE,
 216                  CMDID_WRITE_REG, index, nch->nch_IfNum);
 217     ioerr = psdDoPipe(nch->nch_EP0Pipe, buf, len);
 218     if(ioerr)
 219     {
 220         Printf("Error writing registers 0x%02lx - 0x%02lx, errorcode=%ld\n", index, index+len-1, ioerr);
 221         return FALSE;
 222     }
 223     for(cnt = 0; cnt < len; cnt++)
 224     {
 225         nch->nch_Reg[(index + cnt) & 0xff] = buf[cnt];
 226     }
 227     return TRUE;
 228 }
 229
 230 ULONG ReadReg(struct NepClassSonixcam *nch, ULONG index)
 231 {
 232     LONG ioerr;
 233     UBYTE buf;
 234
 235     //Printf("Reading register 0x%02lx\n", index);
 236     psdPipeSetup(nch->nch_EP0Pipe, URTF_IN|URTF_VENDOR|URTF_INTERFACE,
 237                  CMDID_READ_REG, index, nch->nch_IfNum);
 238     ioerr = psdDoPipe(nch->nch_EP0Pipe, &buf, 1);
 239     if(ioerr)
 240     {
 241         Printf("Error reading register 0x%02lx, errorcode=%ld\n", index, ioerr);
 242         return -1;
 243     }
 244     return buf;
 245 }
 246
 247 BOOL WaitI2C(struct NepClassSonixcam *nch, BOOL read)
 248 {
 249     UWORD cnt = 5;
 250     ULONG reg;
 251     UBYTE cmpval = read ? 0x04 : 0x0c;
 252
 253     do
 254     {
 255         reg = ReadReg(nch, SXREG_I2C_CTRL);
 256         if(reg > 255)
 257         {
 258             return FALSE;
 259         }
 260         if((reg & 0x0c) == cmpval)
 261         {
 262             Printf("%s I2C error %02lx\n", read ? "Read" :  "Write", reg);
 263             return FALSE;
 264         }
 265         if(reg & 0x04)
 266         {
 267             return TRUE;
 268         }
 269         psdDelayMS(3);
 270     } while(--cnt);
 271     PutStr("I2C timeout\n");
 272     return FALSE;
 273 }
 274
 275 BOOL WriteI2C_Raw(struct NepClassSonixcam *nch, UBYTE *cmd)
 276 {
 277     LONG ioerr;
 278     BOOL res;
 279     /*Printf("Writing RAW I2C: %02lx %02lx %02lx %02lx %02lx %02lx %02lx %02lx\n",
 280            cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], cmd[6], cmd[7]);*/
 281
 282     psdPipeSetup(nch->nch_EP0Pipe, URTF_OUT|URTF_VENDOR|URTF_INTERFACE,
 283                  CMDID_WRITE_REG, SXREG_I2C_CTRL, nch->nch_IfNum);
 284     ioerr = psdDoPipe(nch->nch_EP0Pipe, cmd, 8);
 285     if(ioerr)
 286     {
 287         Printf("Error writing I2C Reg 0x%2lx with value 0x%02lx, errorcode=%ld\n", cmd[2], cmd[3], ioerr);
 288         return FALSE;
 289     }
 290     res = WaitI2C(nch, FALSE);
 291     if(!res)
 292     {
 293         Printf("Error writing I2C Reg 0x%2lx with value 0x%02lx during wait\n", cmd[2], cmd[3]);
 294     }
 295     return(res);
 296 }
 297
 298 BOOL WriteI2C(struct NepClassSonixcam *nch, ULONG index, ULONG value)
 299 {
 300     UBYTE cmd[8];
 301
 302     cmd[0] = 0xa0;
 303     cmd[1] = nch->nch_I2CAddr;
 304     cmd[2] = index;
 305     cmd[3] = value;
 306     cmd[4] = 0;
 307     cmd[5] = 0;
 308     cmd[6] = 0;
 309     cmd[7] = 0x17;
 310     return(WriteI2C_Raw(nch, cmd));
 311 }
 312
 313 ULONG ReadI2C(struct NepClassSonixcam *nch, ULONG index)
 314 {
 315     LONG ioerr;
 316     UBYTE cmd[8];
 317
 318     cmd[0] = 0x90;
 319     cmd[1] = nch->nch_I2CAddr;
 320     cmd[2] = index;
 321     cmd[3] = 0;
 322     cmd[4] = 0;
 323     cmd[5] = 0;
 324     cmd[6] = 0;
 325     cmd[7] = 0x10;
 326     psdPipeSetup(nch->nch_EP0Pipe, URTF_OUT|URTF_VENDOR|URTF_INTERFACE,
 327                  CMDID_WRITE_REG, SXREG_I2C_CTRL, nch->nch_IfNum);
 328     ioerr = psdDoPipe(nch->nch_EP0Pipe, cmd, 8);
 329     if(ioerr)
 330     {
 331         Printf("Error reading I2C Reg 0x%2lx (phase 1), errorcode=%ld\n", index, ioerr);
 332         return FALSE;
 333     }
 334     if(!WaitI2C(nch, FALSE))
 335     {
 336         return FALSE;
 337     }
 338
 339     cmd[0] = 0xa2;
 340     psdPipeSetup(nch->nch_EP0Pipe, URTF_OUT|URTF_VENDOR|URTF_INTERFACE,
 341                  CMDID_WRITE_REG, SXREG_I2C_CTRL, nch->nch_IfNum);
 342     ioerr = psdDoPipe(nch->nch_EP0Pipe, cmd, 8);
 343     if(ioerr)
 344     {
 345         Printf("Error reading I2C Reg 0x%2lx (phase 2), errorcode=%ld\n", index, ioerr);
 346         return FALSE;
 347     }
 348
 349     if(!WaitI2C(nch, TRUE))
 350     {
 351         return FALSE;
 352     }
 353
 354     psdPipeSetup(nch->nch_EP0Pipe, URTF_IN|URTF_VENDOR|URTF_INTERFACE,
 355                  CMDID_READ_REG, SXREG_I2C_CTRL, nch->nch_IfNum);
 356     ioerr = psdDoPipe(nch->nch_EP0Pipe, cmd, 8);
 357     if(ioerr)
 358     {
 359         Printf("Error reading I2C Regs 0x%2lx (phase 3), errorcode=%ld\n", index, ioerr);
 360         return FALSE;
 361     }
 362     /*Printf("%02lx %02lx %02lx %02lx %02lx %02lx %02lx %02lx\n",
 363            cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], cmd[6], cmd[7]);*/
 364     return cmd[5];
 365 }
 366
 367 UBYTE initOv7630[] =
 368 {
 369     // 0     1     2     3     4     5     6     7     8     9     a     b     c     d     e     f
 370     0x00, 0x04, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x21, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 371     0x00, 0x00, 0x02, 0x03, 0x0a, 0x28, 0x1e, 0x68, 0xC9, 0x20, 0x1d, 0x10, 0x02, 0x03, 0x0f, 0x0c
 372     //0x00, 0x00, 0x02, 0x03, 0x0a, 0x28, 0x1e, 0x68, 0xc9, 0x50, 0x1d, 0x10, 0x02, 0x03, 0x0f, 0x0c
 373      //shift one pixel 0x02 is 0x01 at start
 374 };
 375
 376 UBYTE ov7630_sensor_init[] =
 377 {
 378     0xa0, 0x21, 0x12, 0x80, 0x00, 0x00, 0x00, 0x10,
 379     0xb0, 0x21, 0x01, 0x77, 0x3a, 0x00, 0x00, 0x10,
 380     0xd0, 0x21, 0x12, 0x78, 0x00, 0x80, 0x34, 0x10,
 381     0xa0, 0x21, 0x1b, 0x04, 0x00, 0x80, 0x34, 0x10,
 382     0xa0, 0x21, 0x20, 0x44, 0x00, 0x80, 0x34, 0x10,
 383     0xa0, 0x21, 0x23, 0xee, 0x00, 0x80, 0x34, 0x10,
 384     0xd0, 0x21, 0x26, 0xa0, 0x9a, 0xa0, 0x30, 0x10,
 385     0xb0, 0x21, 0x2a, 0x80, 0x00, 0xa0, 0x30, 0x10,
 386     0xb0, 0x21, 0x2f, 0x3d, 0x24, 0xa0, 0x30, 0x10,
 387     0xa0, 0x21, 0x32, 0x86, 0x24, 0xa0, 0x30, 0x10,
 388     0xb0, 0x21, 0x60, 0xa9, 0x42, 0xa0, 0x30, 0x10,
 389     0xa0, 0x21, 0x65, 0x00, 0x42, 0xa0, 0x30, 0x10,
 390     0xa0, 0x21, 0x69, 0x38, 0x42, 0xa0, 0x30, 0x10,
 391     0xc0, 0x21, 0x6f, 0x88, 0x0b, 0x00, 0x30, 0x10,
 392     0xc0, 0x21, 0x74, 0x21, 0x8e, 0x00, 0x30, 0x10,
 393     0xa0, 0x21, 0x7d, 0xf7, 0x8e, 0x00, 0x30, 0x10,
 394     0xd0, 0x21, 0x17, 0x1c, 0xbd, 0x06, 0xf6, 0x10,       //
 395     0xa0, 0x21, 0x10, 0x36, 0xbd, 0x06, 0xf6, 0x16,       // exposure
 396     0xa0, 0x21, 0x76, 0x03, 0xbd, 0x06, 0xf6, 0x16,
 397     0xa0, 0x21, 0x11, 0x01, 0xbd, 0x06, 0xf6, 0x16,
 398     0xa0, 0x21, 0x00, 0x10, 0xbd, 0x06, 0xf6, 0x15,       //gain
 399   //0xb0, 0x21, 0x2a, 0xc0, 0x3c, 0x06, 0xf6, 0x1d,//a0 1c,a0 1f,c0 3c frame rate ?line interval from ov6630
 400     0xb0, 0x21, 0x2a, 0xa0, 0x1f, 0x06, 0xf6, 0x10,
 401     0,
 402 };
 403
 404 BOOL SenseOV7631(struct NepClassSonixcam *nch)
 405 {
 406     ULONG pid;
 407     UBYTE *regs = initOv7630;
 408     UBYTE *sensinit = ov7630_sensor_init;
 409
 410     WriteReg(nch, SXREG_CTRL, 0x01); // reset
 411     WriteReg(nch, SXREG_CTRL, 0x00); // release reset
 412     WriteReg(nch, SXREG_VID_CTRL_1, 0x28); // init
 413
 414     nch->nch_I2CAddr = 0x21;
 415
 416         pid = (ReadI2C(nch, 0x0a)<<8)|ReadI2C(nch, 0x0b);
 417     if(pid != 0x7631)
 418     {
 419         return FALSE;
 420     }
 421 #if 0
 422     WriteReg(nch, 0x14, 0x00);
 423     WriteReg(nch, SXREG_VID_CTRL_1, 0x60);
 424     WriteReg(nch, SXREG_VID_CTRL_2, 0x0f);
 425     WriteReg(nch, SXREG_VID_CTRL_3, 0x50); // uncompressed, compressed 0x20
 426
 427     WriteReg(nch, SXREG_WINDOW_LEFT, 1);
 428     WriteReg(nch, SXREG_WINDOW_TOP, 1);
 429     WriteReg(nch, SXREG_WINDOW_WIDTH, 640>>4);
 430     WriteReg(nch, SXREG_WINDOW_HEIGHT, 480>>4);
 431
 432         WriteI2C(nch, 0x12, 0x8d);
 433         WriteI2C(nch, 0x12, 0x0d);
 434         WriteI2C(nch, 0x11, 0x00);
 435         WriteI2C(nch, 0x15, 0x35);
 436         WriteI2C(nch, 0x16, 0x03);
 437         WriteI2C(nch, 0x17, 0x1c);
 438         WriteI2C(nch, 0x18, 0xbd);
 439         WriteI2C(nch, 0x19, 0x06);
 440         WriteI2C(nch, 0x1a, 0xf6);
 441         WriteI2C(nch, 0x1b, 0x04);
 442         WriteI2C(nch, 0x20, 0x44);
 443         WriteI2C(nch, 0x23, 0xee);
 444         WriteI2C(nch, 0x26, 0xa0);
 445         WriteI2C(nch, 0x27, 0x9a);
 446         WriteI2C(nch, 0x28, 0x20);
 447         WriteI2C(nch, 0x29, 0x30);
 448         WriteI2C(nch, 0x2f, 0x3d);
 449         WriteI2C(nch, 0x30, 0x24);
 450         WriteI2C(nch, 0x32, 0x86);
 451         WriteI2C(nch, 0x60, 0xa9);
 452         WriteI2C(nch, 0x61, 0x42);
 453         WriteI2C(nch, 0x65, 0x00);
 454         WriteI2C(nch, 0x69, 0x38);
 455         WriteI2C(nch, 0x6f, 0x88);
 456         WriteI2C(nch, 0x70, 0x0b);
 457         WriteI2C(nch, 0x71, 0x00);
 458         WriteI2C(nch, 0x74, 0x21);
 459         WriteI2C(nch, 0x7d, 0xf7);
 460
 461     WriteReg(nch, SXREG_CTRL, (ULONG) nch->nch_Reg[SXREG_CTRL]|0x04);
 462 #else
 463     WriteReg(nch, SXREG_CTRL,       (ULONG) regs[SXREG_CTRL]);
 464     WriteReg(nch, SXREG_VID_CTRL_1, (ULONG) regs[SXREG_VID_CTRL_1]);
 465     WriteRegs(nch, SXREG_CTRL, 0x1f, &regs[SXREG_CTRL]);
 466
 467     while(*sensinit)
 468     {
 469         WriteI2C_Raw(nch, sensinit);
 470         sensinit += 8;
 471     }
 472
 473     WriteReg(nch, SXREG_WINDOW_LEFT, 2);
 474     WriteReg(nch, SXREG_WINDOW_TOP, 3);
 475     WriteReg(nch, SXREG_WINDOW_WIDTH, 640>>4);
 476     WriteReg(nch, SXREG_WINDOW_HEIGHT, 480>>4);
 477     WriteReg(nch, SXREG_VID_CTRL_1, 0x28);
 478     WriteReg(nch, SXREG_VID_CTRL_2, 0x1F);
 479     WriteReg(nch, SXREG_VID_CTRL_3, 0x50); // 0x33
 480 #endif
 481     return(TRUE);
 482 }
 483
 484 BOOL SenseOV7648(struct NepClassSonixcam *nch)
 485 {
 486     ULONG pid;
 487     UBYTE *regs = initOv7630;
 488     UBYTE *sensinit = ov7630_sensor_init;
 489
 490     WriteReg(nch, SXREG_CTRL, 0x01); // reset
 491     WriteReg(nch, SXREG_CTRL, 0x00); // release reset
 492     WriteReg(nch, SXREG_VID_CTRL_1, 0x28); // init
 493
 494     nch->nch_I2CAddr = 0x21;
 495
 496         pid = (ReadI2C(nch, 0x0a)<<8)|ReadI2C(nch, 0x0b);
 497     if(pid != 0x7648)
 498     {
 499         return FALSE;
 500     }
 501     WriteReg(nch, SXREG_CTRL,       (ULONG) regs[SXREG_CTRL]);
 502     WriteReg(nch, SXREG_VID_CTRL_1, (ULONG) regs[SXREG_VID_CTRL_1]);
 503     WriteRegs(nch, SXREG_CTRL, 0x1f, &regs[SXREG_CTRL]);
 504
 505     while(*sensinit)
 506     {
 507         WriteI2C_Raw(nch, sensinit);
 508         sensinit += 8;
 509     }
 510
 511     WriteReg(nch, SXREG_WINDOW_LEFT, 2);
 512     WriteReg(nch, SXREG_WINDOW_TOP, 3);
 513     WriteReg(nch, SXREG_WINDOW_WIDTH, 640>>4);
 514     WriteReg(nch, SXREG_WINDOW_HEIGHT, 480>>4);
 515     WriteReg(nch, SXREG_VID_CTRL_1, 0x28);
 516     WriteReg(nch, SXREG_VID_CTRL_2, 0x1F);
 517     WriteReg(nch, SXREG_VID_CTRL_3, 0x50); // 0x33
 518     return(TRUE);
 519 }
 520
 521 BOOL SenseHV7131R(struct NepClassSonixcam *nch)
 522 {
 523     ULONG val0;
 524     ULONG val1;
 525
 526     WriteReg(nch, SXREG_CTRL, 0x01); // reset
 527     WriteReg(nch, SXREG_CTRL, 0x00); // release reset
 528     WriteReg(nch, SXREG_VID_CTRL_1, 0x28); // init
 529
 530     //nch->nch_I2CAddr = 0x11;
 531     nch->nch_I2CAddr = 0x5d;
 532
 533         val0 = ReadI2C(nch, 0x00);
 534         val1 = ReadI2C(nch, 0x01);
 535     Printf("PID=0x%02lx, Version=0x%02lx\n", val0, val1);
 536     return FALSE;
 537 }
 538
 539 struct AutoBindData
 540 {
 541     UWORD abd_VendID;
 542     UWORD abd_ProdID;
 543     UWORD abd_BridgeID;
 544 };
 545
 546 struct AutoBindData CamBinds[] =
 547 {
 548         { 0x0c45, 0x6001, BRIDGE_SN9C102 },
 549         { 0x0c45, 0x6005, BRIDGE_SN9C102 },
 550         { 0x0c45, 0x6007, BRIDGE_SN9C102 },
 551         { 0x0c45, 0x6009, BRIDGE_SN9C102 },
 552         { 0x0c45, 0x6011, BRIDGE_SN9C102 },
 553         { 0x0c45, 0x600d, BRIDGE_SN9C102 },
 554         { 0x0c45, 0x6019, BRIDGE_SN9C102 },
 555         { 0x0c45, 0x6024, BRIDGE_SN9C102 },
 556         { 0x0c45, 0x6025, BRIDGE_SN9C102 },
 557         { 0x0c45, 0x6028, BRIDGE_SN9C102 },
 558         { 0x0c45, 0x6029, BRIDGE_SN9C102 },
 559         { 0x0c45, 0x602a, BRIDGE_SN9C102 },
 560         { 0x0c45, 0x602b, BRIDGE_SN9C102 },
 561         { 0x0c45, 0x602c, BRIDGE_SN9C102 },
 562         { 0x0c45, 0x602d, BRIDGE_SN9C102 },
 563         { 0x0c45, 0x602e, BRIDGE_SN9C102 },
 564         { 0x0c45, 0x6030, BRIDGE_SN9C102 },
 565         { 0x0c45, 0x603f, BRIDGE_SN9C102 },
 566
 567         { 0x0c45, 0x6080, BRIDGE_SN9C103 },
 568         { 0x0c45, 0x6082, BRIDGE_SN9C103 },
 569         { 0x0c45, 0x6083, BRIDGE_SN9C103 },
 570         { 0x0c45, 0x6088, BRIDGE_SN9C103 },
 571         { 0x0c45, 0x608a, BRIDGE_SN9C103 },
 572         { 0x0c45, 0x608b, BRIDGE_SN9C103 },
 573         { 0x0c45, 0x608c, BRIDGE_SN9C103 },
 574         { 0x0c45, 0x608e, BRIDGE_SN9C103 },
 575         { 0x0c45, 0x608f, BRIDGE_SN9C103 },
 576         { 0x0c45, 0x60a0, BRIDGE_SN9C103 },
 577         { 0x0c45, 0x60a2, BRIDGE_SN9C103 },
 578         { 0x0c45, 0x60a3, BRIDGE_SN9C103 },
 579         { 0x0c45, 0x60a8, BRIDGE_SN9C103 },
 580         { 0x0c45, 0x60aa, BRIDGE_SN9C103 },
 581         { 0x0c45, 0x60ab, BRIDGE_SN9C103 },
 582         { 0x0c45, 0x60ac, BRIDGE_SN9C103 },
 583         { 0x0c45, 0x60ae, BRIDGE_SN9C103 },
 584         { 0x0c45, 0x60af, BRIDGE_SN9C103 },
 585         { 0x0c45, 0x60b0, BRIDGE_SN9C103 },
 586         { 0x0c45, 0x60b2, BRIDGE_SN9C103 },
 587         { 0x0c45, 0x60b3, BRIDGE_SN9C103 },
 588         { 0x0c45, 0x60b8, BRIDGE_SN9C103 },
 589         { 0x0c45, 0x60ba, BRIDGE_SN9C103 },
 590         { 0x0c45, 0x60bb, BRIDGE_SN9C103 },
 591         { 0x0c45, 0x60bc, BRIDGE_SN9C103 },
 592         { 0x0c45, 0x60be, BRIDGE_SN9C103 },
 593
 594         { 0x045e, 0x00f5, BRIDGE_SN9C105 },
 595         { 0x045e, 0x00f7, BRIDGE_SN9C105 },
 596         { 0x0471, 0x0327, BRIDGE_SN9C105 },
 597         { 0x0471, 0x0328, BRIDGE_SN9C105 },
 598         { 0x0c45, 0x60c0, BRIDGE_SN9C105 },
 599         { 0x0c45, 0x60c2, BRIDGE_SN9C105 },
 600         { 0x0c45, 0x60c8, BRIDGE_SN9C105 },
 601         { 0x0c45, 0x60cc, BRIDGE_SN9C105 },
 602         { 0x0c45, 0x60ea, BRIDGE_SN9C105 },
 603         { 0x0c45, 0x60ec, BRIDGE_SN9C105 },
 604         { 0x0c45, 0x60ef, BRIDGE_SN9C105 },
 605         { 0x0c45, 0x60fa, BRIDGE_SN9C105 },
 606         { 0x0c45, 0x60fb, BRIDGE_SN9C105 },
 607         { 0x0c45, 0x60fc, BRIDGE_SN9C105 },
 608         { 0x0c45, 0x60fe, BRIDGE_SN9C105 },
 609
 610         { 0x0458, 0x7025, BRIDGE_SN9C120 },
 611         { 0x0458, 0x7034, BRIDGE_SN9C120 },
 612         { 0x0c45, 0x6102, BRIDGE_SN9C120 },
 613         { 0x0c45, 0x6108, BRIDGE_SN9C120 },
 614         { 0x0c45, 0x610f, BRIDGE_SN9C120 },
 615         { 0x0c45, 0x6130, BRIDGE_SN9C120 },
 616         { 0x0c45, 0x6138, BRIDGE_SN9C120 },
 617         { 0x0c45, 0x613a, BRIDGE_SN9C120 },
 618         { 0x0c45, 0x613b, BRIDGE_SN9C120 },
 619         { 0x0c45, 0x613c, BRIDGE_SN9C120 },
 620         { 0x0c45, 0x613e, BRIDGE_SN9C120 },
 621     { 0x0000, 0x0000, 0 }
 622 };
 623
 624
 625 struct NepClassSonixcam * SetupSonixcam(void)
 626 {
 627     struct NepClassSonixcam *nch;
 628     struct PsdDevice *pd = NULL;
 629     struct PsdAppBinding *pab;
 630     ULONG unit;
 631     struct AutoBindData *abd;
 632
 633     if(ArgsArray[ARGS_UNIT])
 634     {
 635         unit = *((ULONG *) ArgsArray[ARGS_UNIT]);
 636     } else {
 637         unit = 0;
 638     }
 639     do
 640     {
 641         abd = CamBinds;
 642         do
 643         {
 644             pd = psdFindDevice(pd,
 645                                DA_VendorID, abd->abd_VendID,
 646                                DA_ProductID, abd->abd_ProdID,
 647                                TAG_END);
 648             if(!pd)
 649             {
 650                 abd++;
 651                 if(!abd->abd_VendID)
 652                 {
 653                     break;
 654                 }
 655             } else {
 656                 if(!unit)
 657                 {
 658                     break;
 659                 }
 660                 unit--;
 661             }
 662         } while(TRUE);
 663
 664         if(!pd)
 665         {
 666             PutStr("No Sonixcam found!\n");
 667             return(NULL);
 668         }
 669         if((nch = psdAllocVec(sizeof(struct NepClassSonixcam))))
 670         {
 671             nch->nch_Device = pd;
 672             nch->nch_BridgeID= abd->abd_BridgeID;
 673             nch->nch_ReleaseHook.h_Entry = (APTR) releasehook;
 674
 675             pab = psdClaimAppBinding(ABA_Device, pd,
 676                                      ABA_ReleaseHook, &nch->nch_ReleaseHook,
 677                                      ABA_UserData, nch,
 678                                      ABA_ForceRelease, TRUE,
 679                                      TAG_END);
 680             if(pab)
 681             {
 682                 if(AllocSonixcam(nch))
 683                 {
 684                     return(nch);
 685                 } else {
 686                     PutStr("Couldn't allocate Sonixcam...\n");
 687                 }
 688                 psdReleaseAppBinding(pab);
 689             } else {
 690                 PutStr("Couldn't claim binding!\n");
 691             }
 692             psdFreeVec(nch);
 693         }
 694         PutStr("Hohum...\n");
 695     } while(FALSE);
 696     return(NULL);
 697 }
 698
 699 struct RastPort * SetupText(void)
 700 {
 701     STRPTR text = (STRPTR) ArgsArray[ARGS_TEXT];
 702     InitRastPort(&fontrp);
 703     if(ArgsArray[ARGS_FONT])
 704     {
 705         avenirta.ta_Name = (STRPTR) ArgsArray[ARGS_FONT];
 706         if(ArgsArray[ARGS_FONTSIZE])
 707         {
 708             avenirta.ta_YSize = *((ULONG *) ArgsArray[ARGS_FONTSIZE]);
 709         } else {
 710             avenirta.ta_YSize = 8;
 711         }
 712         avenirta.ta_Style = 0L;
 713         avenirta.ta_Flags = 0L;
 714         if(!(avenirfont = OpenDiskFont(&avenirta)))
 715         {
 716             Printf("Couldn't open font!\n");
 717         } else {
 718             SetFont(&fontrp, avenirfont);
 719         }
 720     }
 721     tlength = TextLength(&fontrp, text, (ULONG) strlen(text));
 722     theight = fontrp.Font->tf_YSize;
 723     if(!(fontbm = AllocBitMap(tlength, theight, 1L, BMF_CLEAR, NULL)))
 724     {
 725         Printf("Couldn't allocate font bitmap memory (%ldx%ld)", tlength, theight);
 726         return(NULL);
 727     }
 728     fontrp.BitMap = fontbm;
 729     //printf("String: %s\nLength: %d\n",(char *)argsarray[ARGS_TEXT], tlength);
 730     Move(&fontrp, 0, (LONG) fontrp.Font->tf_Baseline);
 731     Text(&fontrp, text, (ULONG) strlen(text));
 732     return(&fontrp);
 733 }
 734
 735 void FreeText(void)
 736 {
 737     if(fontbm)
 738     {
 739         FreeBitMap(fontbm);
 740         fontbm = NULL;
 741     }
 742     if(avenirfont)
 743     {
 744         CloseFont(avenirfont);
 745         avenirfont = NULL;
 746     }
 747 }
 748
 749 void PasteText(struct SCImageHeader *scih, UBYTE *output)
 750 {
 751     LONG x, y;
 752     LONG tarx, tary;
 753     LONG vw = scih->scih_ImgWidth;
 754     LONG vw3 = vw+vw+vw;
 755     UBYTE *op;
 756     WORD pix;
 757     if(tlength < vw-2)
 758     {
 759         tarx = (vw-tlength) >> 1;
 760     } else {
 761         tarx = 1;
 762     }
 763     tary = (scih->scih_ImgHeight-theight-theight);
 764     for(y = 0; y < theight; y++)
 765     {
 766         for(x = 0; (x < tlength) && (x+tarx+2 < vw); x++)
 767         {
 768             pix = ReadPixel(&fontrp, x, y);
 769             if(pix)
 770             {
 771                 op = &output[((tary+y)*vw+tarx+x)*3];
 772                 op[-vw3] >>= 1;
 773                 op[-vw3+1] >>= 1;
 774                 op[-vw3+2] >>= 1;
 775                 op[-3] >>= 1;
 776                 op[-2] >>= 1;
 777                 op[-1] >>= 1;
 778                 op[3] >>= 1;
 779                 op[4] >>= 1;
 780                 op[5] >>= 1;
 781                 op[vw3] >>= 1;
 782                 op[vw3+1] >>= 1;
 783                 op[vw3+2] >>= 1;
 784             }
 785         }
 786     }
 787     for(y = 0; y < theight; y++)
 788     {
 789         for(x = 0; (x < tlength) && (x+tarx+2 < vw); x++)
 790         {
 791             pix = ReadPixel(&fontrp, x, y);
 792             if(pix)
 793             {
 794                 op = &output[((tary+y)*vw+tarx+x)*3];
 795                 *op++ = 255;
 796                 *op++ = 255;
 797                 *op = 255;
 798             }
 799         }
 800     }
 801 }
 802
 803 struct NepClassSonixcam * AllocSonixcam(struct NepClassSonixcam *nch)
 804 {
 805     BOOL foundsensor;
 806     nch->nch_Task = FindTask(NULL);
 807
 808     nch->nch_Interface = psdFindInterface(nch->nch_Device, NULL,
 809                                           IFA_AlternateNum, 8,
 810                                           TAG_END);
 811     if(!nch->nch_Interface)
 812     {
 813         PutStr("No interfaces?\n");
 814         return(NULL);
 815     }
 816
 817     psdGetAttrs(PGA_INTERFACE, nch->nch_Interface,
 818                  IFA_InterfaceNum, &nch->nch_IfNum,
 819                  TAG_END);
 820     if((nch->nch_TaskMsgPort = CreateMsgPort()))
 821     {
 822         nch->nch_ImgDoneSig = AllocSignal(-1L);
 823         if((nch->nch_EP0Pipe = psdAllocPipe(nch->nch_Device, nch->nch_TaskMsgPort, NULL)))
 824         {
 825             psdSetAltInterface(nch->nch_EP0Pipe, nch->nch_Interface);
 826             nch->nch_IsoEP = psdFindEndpoint(nch->nch_Interface, NULL,
 827                                              EA_IsIn, TRUE,
 828                                              EA_TransferType, USEAF_ISOCHRONOUS,
 829                                              TAG_END);
 830             if(nch->nch_IsoEP)
 831             {
 832                 nch->nch_InReqHook.h_Entry = (HOOKFUNC) nInReqHook;
 833                 nch->nch_InReqHook.h_Data = nch;
 834                 nch->nch_InDoneHook.h_Entry = (HOOKFUNC) nInDoneHook;
 835                 nch->nch_InDoneHook.h_Data = nch;
 836                 nch->nch_RTIso = psdAllocRTIsoHandler(nch->nch_IsoEP,
 837                                                       RTA_InRequestHook, &nch->nch_InReqHook,
 838                                                       RTA_InDoneHook, &nch->nch_InDoneHook,
 839                                                       TAG_END);
 840                 if(nch->nch_RTIso)
 841                 {
 842                     foundsensor = SenseOV7631(nch);
 843                     if(!foundsensor)
 844                     {
 845                         foundsensor = SenseHV7131R(nch);
 846                     }
 847
 848                     if(foundsensor)
 849                     {
 850                         switch(nch->nch_BridgeID)
 851                         {
 852                             case BRIDGE_SN9C102:
 853                                 nch->nch_HeaderSize = 12;
 854                                 break;
 855
 856                             case BRIDGE_SN9C103:
 857                                 nch->nch_HeaderSize = 18;
 858                                 break;
 859
 860                             case BRIDGE_SN9C105:
 861                             case BRIDGE_SN9C120:
 862                                 nch->nch_HeaderSize = 62;
 863                                 break;
 864                         }
 865
 866                         nch->nch_RawBufSize = 320*240 + nch->nch_HeaderSize;
 867                         nch->nch_RawBuf[0] = psdAllocVec(nch->nch_RawBufSize * 3);
 868                         if(nch->nch_RawBuf[0])
 869                         {
 870                             nch->nch_RawBuf[1] = nch->nch_RawBuf[0] + nch->nch_RawBufSize;
 871                             nch->nch_RawBuf[2] = nch->nch_RawBuf[1] + nch->nch_RawBufSize;
 872                             nch->nch_ImgBufSize = 320*240*3;
 873                             nch->nch_ImgBuf = psdAllocVec(nch->nch_ImgBufSize);
 874                             if(nch->nch_ImgBuf)
 875                             {
 876                                 return(nch);
 877                             } else {
 878                                 Printf("Couldn't allocate %ld bytes of memory.\n", nch->nch_ImgBufSize);
 879                             }
 880                             psdFreeVec(nch->nch_RawBuf[0]);
 881                         } else {
 882                             Printf("Couldn't allocate %ld bytes of memory.\n", nch->nch_RawBufSize * 3);
 883                         }
 884                     } else {
 885                         PutStr("Failed to init sensor\n");
 886                     }
 887                     psdFreeRTIsoHandler(nch->nch_RTIso);
 888                 } else {
 889                     PutStr("Couldn't allocate RT Iso Handler.\n");
 890                 }
 891             } else {
 892                 PutStr("No iso endpoint?\n");
 893             }
 894             psdFreePipe(nch->nch_EP0Pipe);
 895         } else {
 896             PutStr("Couldn't allocate default pipe\n");
 897         }
 898         FreeSignal(nch->nch_ImgDoneSig);
 899         DeleteMsgPort(nch->nch_TaskMsgPort);
 900     }
 901     return(NULL);
 902 }
 903
 904 void FreeSonixcam(struct NepClassSonixcam *nch)
 905 {
 906     APTR pab;
 907     psdFreeVec(nch->nch_ImgBuf);
 908     psdFreeVec(nch->nch_RawBuf[0]);
 909     psdFreeRTIsoHandler(nch->nch_RTIso);
 910     psdGetAttrs(PGA_DEVICE, nch->nch_Device,
 911                 DA_Binding, &pab,
 912                 TAG_END);
 913     psdReleaseAppBinding(pab);
 914     psdFreePipe(nch->nch_EP0Pipe);
 915     FreeSignal(nch->nch_ImgDoneSig);
 916     DeleteMsgPort(nch->nch_TaskMsgPort);
 917     psdFreeVec(nch);
 918 }
 919
 920 /**************************************************************************/
 921
 922 void bayer_unshuffle(struct SCImageHeader *scih, UBYTE *raw, UBYTE *output)
 923 {
 924     ULONG x, y;
 925     ULONG vw = scih->scih_ImgWidth;
 926     ULONG vh = scih->scih_ImgHeight;
 927     ULONG w3 = vw + vw +vw;
 928     ULONG w12 = vw>>1;
 929     UBYTE *oline;
 930
 931 /* Byte Order. Each cell is one byte.
 932    start + 0:  B00 G01 B02 G03
 933    start + 4:  G10 R11 G12 R13
 934    start + 8:  B20 G21 B22 G23
 935    start + 12: G30 R31 G32 R33 */
 936
 937 /* Byte Order. Each cell is one byte.
 938    start + 0:  G00 B01 G02 B03
 939    start + 4:  R10 G11 R12 G13
 940    start + 8:  G20 B21 G22 B23
 941    start + 12: R30 G31 R32 G33 */
 942
 943     // RGB
 944     for(y = 0; y < vh; y++)
 945     {
 946         oline = output + 1 - (y & 1); // start with green on even, red on odd
 947         x = w12;
 948         do
 949         {
 950             *oline = *raw++; // green on even, red on odd
 951             oline += 4;
 952             *oline = *raw++; // blue on even, green on odd
 953             oline += 2;
 954         } while(--x);
 955         output += w3;
 956     }
 957 }
 958
 959 void bayer_demosaic(struct SCImageHeader *scih, UBYTE *output)
 960 {
 961     LONG x, y;
 962     UBYTE *op;
 963
 964 #define vw3 (320*3)
 965
 966     op = output + 3 + vw3;
 967     y = (240 - 2)>>1;
 968     do
 969     {
 970         x = (320 - 2)>>1;
 971         do
 972         {
 973              /* 11 green. red lr, blue tb */
 974              *op = (((UWORD) op[-3]) + ((UWORD) op[3])) >> 1; /* Set red */
 975              op[2] = (((UWORD) op[2-vw3]) + ((UWORD) op[2+vw3]) + 1) >> 1; /* Set blue */
 976              op += 3;
 977              /* 10 red. green lrtb, blue diagonals */
 978              op[1] = (((UWORD) op[-2]) + ((UWORD) op[4]) +
 979                      ((UWORD) op[1-vw3]) + ((UWORD) op[1+vw3]) + 2) >> 2; /* Set green */
 980              op[2] = (((UWORD) op[-1-vw3]) + ((UWORD) op[5-vw3]) +
 981                      ((UWORD) op[-1+vw3]) + ((UWORD) op[5+vw3]) + 2) >> 2; /* Set blue */
 982              op += 3;
 983         } while(--x);
 984         op += 6;
 985         x = (320 - 2)>>1;
 986         do
 987         {
 988              /* 01 blue. green lrtb, red diagonals */
 989              op[1] = (((UWORD) op[-2]) + ((UWORD) op[4]) +
 990                      ((UWORD) op[1-vw3]) + ((UWORD) op[1+vw3]) + 2) >> 2; /* Set green */
 991              *op = (((UWORD) op[-3-vw3]) + ((UWORD) op[3-vw3]) +
 992                    ((UWORD) op[-3+vw3]) + ((UWORD) op[3+vw3]) + 2) >> 2; /* Set red */
 993              op += 3;
 994              /* 00 green. blue lr, red tb */
 995              op[2] = (((UWORD) op[-1]) + ((UWORD) op[5]) + 1) >> 1; /* Set blue */
 996              *op = (((UWORD) op[-vw3]) + ((UWORD) op[vw3]) + 1) >> 1; /* Set red */
 997              op += 3;
 998         } while(--x);
 999         op += 6;
1000     } while(--y);
1001     // do border
1002     x = 320;
1003     op = output;
1004     do
1005     {
1006         *op++ = 0;
1007         *op++ = 0;
1008         *op++ = 0;
1009     } while(--x);
1010
1011     y = 238;
1012     do
1013     {
1014         *op++ = 0;
1015         *op++ = 0;
1016         *op = 0;
1017         op += vw3-5;
1018         *op++ = 0;
1019         *op++ = 0;
1020         *op++ = 0;
1021     } while(--y);
1022
1023     x = 320;
1024     do
1025     {
1026         *op++ = 0;
1027         *op++ = 0;
1028         *op++ = 0;
1029     } while(--x);
1030 #undef vw3
1031 }
1032
1033 void gammacorrection(struct SCImageHeader *scih, UBYTE *output)
1034 {
1035     ULONG cnt = scih->scih_ImgWidth*scih->scih_ImgHeight;
1036     while(cnt--)
1037     {
1038         *output = gammaredtab[*output];
1039         output++;
1040         *output = gammagreentab[*output];
1041         output++;
1042         *output = gammabluetab[*output];
1043         output++;
1044     }
1045 }
1046
1047 void sharpen5x5(struct SCImageHeader *scih, UBYTE *input, UBYTE *output)
1048 {
1049     LONG x, y;
1050     LONG vw = scih->scih_ImgWidth;
1051     LONG vw3 = vw+vw+vw;
1052     LONG vw6 = vw*6;
1053     LONG vh = scih->scih_ImgHeight;
1054     LONG linem2[3];
1055     LONG linem1[3];
1056     LONG linep1[3];
1057     LONG linep2[3];
1058     LONG val[3];
1059
1060     UBYTE *op;
1061     UBYTE *oop;
1062     for(y = 2; y < vh-2; y++)
1063     {
1064         op = &input[((y-2)*vw)*3];
1065         linem2[0] = *op++;  // -2
1066         linem2[1] = *op++;
1067         linem2[2] = *op++;
1068         linem2[0] += *op++; // -1
1069         linem2[1] += *op++;
1070         linem2[2] += *op++;
1071         linem2[0] += *op++; // 0
1072         linem2[1] += *op++;
1073         linem2[2] += *op++;
1074         linem2[0] += *op++; // 1
1075         linem2[1] += *op++;
1076         linem2[2] += *op++;
1077         linem2[0] += *op++; // 2
1078         linem2[1] += *op++;
1079         linem2[2] += *op;
1080
1081         op = &input[((y-1)*vw+1)*3];
1082         linem1[0] = *op++;  // -1
1083         linem1[1] = *op++;
1084         linem1[2] = *op++;
1085         linem1[0] += *op++; // 0
1086         linem1[1] += *op++;
1087         linem1[2] += *op++;
1088         linem1[0] += *op++; // 1
1089         linem1[1] += *op++;
1090         linem1[2] += *op++;
1091         linem1[0] += linem1[0]+linem1[0];
1092         linem1[1] += linem1[1]+linem1[1];
1093         linem1[2] += linem1[2]+linem1[2];
1094         linem1[0] += *op++; // 2
1095         linem1[1] += *op++;
1096         linem1[2] += *op++;
1097         linem1[0] += op[-15]; // -2
1098         linem1[1] += op[-14];
1099         linem1[2] += op[-13];
1100
1101         op = &input[((y+1)*vw+1)*3];
1102         linep1[0] = *op++;  // -1
1103         linep1[1] = *op++;
1104         linep1[2] = *op++;
1105         linep1[0] += *op++; // 0
1106         linep1[1] += *op++;
1107         linep1[2] += *op++;
1108         linep1[0] += *op++; // 1
1109         linep1[1] += *op++;
1110         linep1[2] += *op++;
1111         linep1[0] += linep1[0]+linep1[0];
1112         linep1[1] += linep1[1]+linep1[1];
1113         linep1[2] += linep1[2]+linep1[2];
1114         linep1[0] += *op++; // 2
1115         linep1[1] += *op++;
1116         linep1[2] += *op++;
1117         linep1[0] += op[-15]; // -2
1118         linep1[1] += op[-14];
1119         linep1[2] += op[-13];
1120
1121         op = &input[((y+2)*vw)*3];
1122         linep2[0] = *op++;  // -2
1123         linep2[1] = *op++;
1124         linep2[2] = *op++;
1125         linep2[0] += *op++; // -1
1126         linep2[1] += *op++;
1127         linep2[2] += *op++;
1128         linep2[0] += *op++; // 0
1129         linep2[1] += *op++;
1130         linep2[2] += *op++;
1131         linep2[0] += *op++; // 1
1132         linep2[1] += *op++;
1133         linep2[2] += *op++;
1134         linep2[0] += *op++; // 2
1135         linep2[1] += *op++;
1136         linep2[2] += *op;
1137
1138         op = &input[(y*vw + 2)*3];
1139         oop = &output[(y*vw + 2)*3];
1140         for(x = 2; x < vw-2; x++) /* work out pixel type */
1141         {
1142             /* Central line */
1143             val[0] = op[-3] + op[3];
1144             val[1] = op[-2] + op[4];
1145             val[2] = op[-1] + op[5];
1146             val[0] += val[0] + val[0] + op[-6] + op[6] + linem2[0] + linem1[0] + linep1[0] + linep2[0];
1147             val[1] += val[1] + val[1] + op[-5] + op[7] + linem2[1] + linem1[1] + linep1[1] + linep2[1];
1148             val[2] += val[2] + val[2] + op[-4] + op[8] + linem2[2] + linem1[2] + linep1[2] + linep2[2];
1149             val[0] -= op[0] * 56;
1150             val[1] -= op[1] * 56;
1151             val[2] -= op[2] * 56;
1152 #define MAXVAL 4080
1153
1154             *oop++ = (val[0] > 0) ? 0 : ((val[0] < -MAXVAL) ? 255 : (-val[0]+8)>>4);
1155             *oop++ = (val[1] > 0) ? 0 : ((val[1] < -MAXVAL) ? 255 : (-val[1]+8)>>4);
1156             *oop++ = (val[2] > 0) ? 0 : ((val[2] < -MAXVAL) ? 255 : (-val[2]+8)>>4);
1157
1158             /* Update line y-2 */
1159             linem2[0] -= op[-vw6-6];
1160             linem2[0] += op[-vw6+9];
1161             linem2[1] -= op[-vw6-5];
1162             linem2[1] += op[-vw6+10];
1163             linem2[2] -= op[-vw6-4];
1164             linem2[2] += op[-vw6+11];
1165
1166             /* Update line y-1 */
1167             linem1[0] -= op[-vw3-6];
1168             linem1[0] -= op[-vw3-3]<<1;
1169             linem1[0] += op[-vw3+6]<<1;
1170             linem1[0] += op[-vw3+9];
1171             linem1[1] -= op[-vw3-5];
1172             linem1[1] -= op[-vw3-2]<<1;
1173             linem1[1] += op[-vw3+7]<<1;
1174             linem1[1] += op[-vw3+10];
1175             linem1[2] -= op[-vw3-4];
1176             linem1[2] -= op[-vw3-1]<<1;
1177             linem1[2] += op[-vw3+8]<<1;
1178             linem1[2] += op[-vw3+11];
1179
1180             /* Update line y+1 */
1181             linep1[0] -= op[vw3-6];
1182             linep1[0] -= op[vw3-3]<<1;
1183             linep1[0] += op[vw3+6]<<1;
1184             linep1[0] += op[vw3+9];
1185             linep1[1] -= op[vw3-5];
1186             linep1[1] -= op[vw3-2]<<1;
1187             linep1[1] += op[vw3+7]<<1;
1188             linep1[1] += op[vw3+10];
1189             linep1[2] -= op[vw3-4];
1190             linep1[2] -= op[vw3-1]<<1;
1191             linep1[2] += op[vw3+8]<<1;
1192             linep1[2] += op[vw3+11];
1193
1194             /* Update line y-2 */
1195             linep2[0] -= op[vw6-6];
1196             linep2[0] += op[vw6+9];
1197             linep2[1] -= op[vw6-5];
1198             linep2[1] += op[vw6+10];
1199             linep2[2] -= op[vw6-4];
1200             linep2[2] += op[vw6+11];
1201             op += 3;
1202         }
1203     }
1204 }
1205
1206 APTR GetVideoSnap(struct NepClassSonixcam *nch, struct SCImageHeader *scih)
1207 {
1208     LONG ioerr;
1209     UBYTE *newimgbuf;
1210     ULONG sigs;
1211     ULONG lastdone;
1212
1213     scih->scih_ImgWidth = 320;
1214     scih->scih_ImgHeight = 240;
1215     scih->scih_ImgSize = scih->scih_ImgWidth * scih->scih_ImgHeight * 3;
1216
1217     nch->nch_BufState[2] = nch->nch_BufState[1] = nch->nch_BufState[0] = BUF_EMPTY;
1218
1219     nch->nch_CurrIsoBuf = nch->nch_RawBuf[0];
1220     nch->nch_IsoBufNum = 0;
1221     nch->nch_IsoBufPos = 0;
1222
1223     ioerr = psdStartRTIso(nch->nch_RTIso);
1224     if(ioerr)
1225     {
1226         Printf("Error starting RT Iso errorcode=%ld\n", ioerr);
1227         return(NULL);
1228     }
1229
1230     do
1231     {
1232         sigs = Wait((1UL<<nch->nch_ImgDoneSig)|SIGBREAKF_CTRL_C);
1233         lastdone = nch->nch_LastDoneNum;
1234         if(nch->nch_BufState[lastdone] == BUF_READY)
1235         {
1236             break;
1237         }
1238     } while(!(sigs & SIGBREAKF_CTRL_C));
1239
1240     ioerr = psdStopRTIso(nch->nch_RTIso);
1241
1242     if(sigs & SIGBREAKF_CTRL_C)
1243     {
1244         return(NULL);
1245     }
1246
1247     nch->nch_BufState[lastdone] = BUF_BUSY;
1248     bayer_unshuffle(scih, nch->nch_RawBuf[lastdone] + nch->nch_HeaderSize, nch->nch_ImgBuf);
1249     nch->nch_BufState[lastdone] = BUF_EMPTY;
1250     bayer_demosaic(scih, nch->nch_ImgBuf);
1251
1252     if(ArgsArray[ARGS_SHARPEN])
1253     {
1254         newimgbuf = psdAllocVec(nch->nch_ImgBufSize);
1255         if(newimgbuf)
1256         {
1257             sharpen5x5(scih, nch->nch_ImgBuf, newimgbuf);
1258             psdFreeVec(nch->nch_ImgBuf);
1259             nch->nch_ImgBuf = newimgbuf;
1260         }
1261     }
1262     if(ArgsArray[ARGS_GAMMA])
1263     {
1264         gammacorrection(scih, nch->nch_ImgBuf);
1265     }
1266     if(ArgsArray[ARGS_TEXT])
1267     {
1268         PasteText(scih, nch->nch_ImgBuf);
1269     }
1270     return(nch->nch_ImgBuf);
1271 }
1272
1273 int main(int argc, char *argv[])
1274 {
1275     struct NepClassSonixcam *nch;
1276     struct SCImageHeader scih;
1277     BPTR outfile;
1278     UBYTE *imgbuf;
1279     ULONG sigs;
1280     char buf[256];
1281     ULONG imgcount;
1282
1283     if(!(ArgsHook = ReadArgs(template, ArgsArray, NULL)))
1284     {
1285         PutStr("Wrong arguments!\n");
1286         return(RETURN_FAIL);
1287     }
1288     ps = OpenLibrary("poseidon.library", 4);
1289     if(!ps)
1290     {
1291         FreeArgs(ArgsHook);
1292         return(RETURN_FAIL);
1293     }
1294     if(ArgsArray[ARGS_TEXT])
1295     {
1296         if(!(SetupText()))
1297         {
1298             FreeArgs(ArgsHook);
1299             CloseLibrary(ps);
1300             return(RETURN_ERROR);
1301         }
1302     }
1303     if(!(nch = SetupSonixcam()))
1304     {
1305         FreeText();
1306         FreeArgs(ArgsHook);
1307         CloseLibrary(ps);
1308         return(RETURN_ERROR);
1309     }
1310     CreateGammaTab();
1311     imgcount = 0;
1312     do
1313     {
1314          /*PutStr("Waiting for CTRL-C before downloading.\n"
1315                 "This is your chance to remove the cam to check if the machine crashes.\n");
1316          Wait(SIGBREAKF_CTRL_C); */
1317          imgbuf = GetVideoSnap(nch, &scih);
1318          if(imgbuf)
1319          {
1320              psdSafeRawDoFmt(buf, 256, (STRPTR) ArgsArray[ARGS_TO], imgcount);
1321              outfile = Open(buf, MODE_NEWFILE);
1322              if(outfile)
1323              {
1324                  FPrintf(outfile, "P6\n%ld %ld\n255\n", scih.scih_ImgWidth, scih.scih_ImgHeight);
1325                  Flush(outfile);
1326                  Write(outfile, imgbuf, scih.scih_ImgSize);
1327                  Close(outfile);
1328                  Printf("Wrote image into '%s'.\n", buf);
1329                  imgcount++;
1330              } else {
1331                  Printf("Could not open file '%s' for writing!\n", buf);
1332              }
1333          } else {
1334              break;
1335          }
1336          /*PutStr("Finished. Waiting for another CTRL-C.\n");*/
1337          sigs = SetSignal(0, 0);
1338          if(sigs & SIGBREAKF_CTRL_C)
1339          {
1340              break;
1341          }
1342          if(ArgsArray[ARGS_INTERVAL])
1343          {
1344              if(ArgsArray[ARGS_UPTO])
1345              {
1346                  if(imgcount > *((ULONG *) ArgsArray[ARGS_UPTO]))
1347                  {
1348                      break;
1349                  }
1350              }
1351              Delay(*((ULONG *) ArgsArray[ARGS_INTERVAL]));
1352          } else {
1353              if(ArgsArray[ARGS_UPTO])
1354              {
1355                  if(imgcount > *((ULONG *) ArgsArray[ARGS_UPTO]))
1356                  {
1357                      break;
1358                  }
1359              } else {
1360                  break;
1361              }
1362          }
1363          sigs = SetSignal(0, 0);
1364          if(sigs & SIGBREAKF_CTRL_C)
1365          {
1366              break;
1367          }
1368     } while(TRUE);
1369     FreeText();
1370     FreeSonixcam(nch);
1371     FreeArgs(ArgsHook);
1372     CloseLibrary(ps);
1373     return(RETURN_OK);
1374 }