1 /* 2 * Copyright (c) 2026 Abdelkader Boudih <freebsd@seuros.com> 3 * 4 * SPDX-License-Identifier: BSD-2-Clause 5 */ 6 7 /* 8 * fwcam(4) - IIDC 1394-based Digital Camera driver 9 * 10 * Implements the IIDC v1.30 specification (TA Document 1999023) for 11 * FireWire digital cameras. 12 */ 13 14 #include <sys/param.h> 15 #include <sys/systm.h> 16 #include <sys/module.h> 17 #include <sys/bus.h> 18 #include <sys/kernel.h> 19 #include <sys/conf.h> 20 #include <sys/malloc.h> 21 #include <sys/lock.h> 22 #include <sys/mutex.h> 23 #include <sys/sysctl.h> 24 #include <sys/taskqueue.h> 25 #include <sys/fcntl.h> 26 #include <sys/poll.h> 27 #include <sys/selinfo.h> 28 #include <sys/uio.h> 29 #include <sys/mbuf.h> 30 31 #include <dev/firewire/firewire.h> 32 #include <dev/firewire/firewirereg.h> 33 #include <dev/firewire/iec13213.h> 34 #include <dev/firewire/fwcam.h> 35 #include <dev/firewire/fw_helpers.h> 36 37 static MALLOC_DEFINE(M_FWCAM, "fwcam", "IIDC FireWire Camera"); 38 39 static int debug = 0; 40 static int iso_channel = 0; 41 SYSCTL_DECL(_hw_firewire); 42 static SYSCTL_NODE(_hw_firewire, OID_AUTO, fwcam, CTLFLAG_RD | CTLFLAG_MPSAFE, 43 0, "IIDC Camera"); 44 SYSCTL_INT(_hw_firewire_fwcam, OID_AUTO, debug, CTLFLAG_RWTUN, &debug, 0, 45 "fwcam debug level"); 46 SYSCTL_INT(_hw_firewire_fwcam, OID_AUTO, iso_channel, CTLFLAG_RWTUN, 47 &iso_channel, 0, "ISO channel for isochronous receive (default 0)"); 48 49 #define FWCAM_DEBUG(lev, fmt, ...) \ 50 do { \ 51 if (debug >= (lev)) \ 52 printf("fwcam: " fmt, ## __VA_ARGS__); \ 53 } while (0) 54 55 static int fwcam_probe(device_t); 56 static int fwcam_attach(device_t); 57 static int fwcam_detach(device_t); 58 static void fwcam_probe_task(void *, int); 59 static int fwcam_iso_start(struct fwcam_softc *); 60 static void fwcam_iso_stop(struct fwcam_softc *); 61 static void fwcam_iso_input(struct fw_xferq *); 62 63 static d_open_t fwcam_cdev_open; 64 static d_close_t fwcam_cdev_close; 65 static d_read_t fwcam_cdev_read; 66 static d_poll_t fwcam_cdev_poll; 67 static d_ioctl_t fwcam_cdev_ioctl; 68 69 static struct cdevsw fwcam_cdevsw = { 70 .d_version = D_VERSION, 71 .d_flags = D_TRACKCLOSE, 72 .d_open = fwcam_cdev_open, 73 .d_close = fwcam_cdev_close, 74 .d_read = fwcam_cdev_read, 75 .d_poll = fwcam_cdev_poll, 76 .d_ioctl = fwcam_cdev_ioctl, 77 .d_name = "fwcam", 78 }; 79 80 /* 81 * Search a CSR directory for the IIDC command base register (key 0x40). 82 * The iSight places cmd_base inside a logical_unit_directory nested 83 * within the IIDC unit directory, so we recurse into sub-directories 84 * up to max_depth levels. 85 */ 86 static uint32_t 87 fwcam_search_dir_cmd_base(uint32_t *csrrom, uint32_t dir_qoff, 88 uint32_t rom_quads, int max_depth) 89 { 90 struct csrdirectory *dir; 91 struct csrreg *reg; 92 uint32_t dir_len, sub_qoff, val; 93 int i; 94 95 if (dir_qoff >= rom_quads || max_depth <= 0) 96 return (0); 97 dir = (struct csrdirectory *)&csrrom[dir_qoff]; 98 dir_len = dir->crc_len; 99 if (dir_qoff + 1 + dir_len > rom_quads) 100 return (0); 101 102 for (i = 0; i < (int)dir_len; i++) { 103 if (dir_qoff + 1 + i >= rom_quads) 104 break; 105 reg = &dir->entry[i]; 106 if (reg->key == IIDC_CROM_CMD_BASE && reg->val != 0) 107 return (reg->val); 108 if ((reg->key & CSRTYPE_MASK) == CSRTYPE_D) { 109 sub_qoff = dir_qoff + 1 + i + reg->val; 110 val = fwcam_search_dir_cmd_base(csrrom, sub_qoff, 111 rom_quads, max_depth - 1); 112 if (val != 0) 113 return (val); 114 } 115 } 116 return (0); 117 } 118 119 /* 120 * Extract IIDC command base register from the unit directory. 121 * Searches the unit directory and any sub-directories (the iSight 122 * places cmd_base inside a logical_unit_directory). 123 */ 124 static uint32_t 125 fwcam_find_iidc_cmd_base(struct fw_unit *unit) 126 { 127 128 return (fwcam_search_dir_cmd_base(unit->fwdev->csrrom, 129 unit->dir_offset / 4, CSRROMSIZE / 4, 2)); 130 } 131 132 static int 133 fwcam_read_quadlet(struct fwcam_softc *sc, uint32_t offset, uint32_t *val) 134 { 135 uint16_t dst; 136 uint8_t spd; 137 int err; 138 139 FWCAM_LOCK(sc); 140 if (sc->fwdev == NULL) { 141 FWCAM_UNLOCK(sc); 142 return (ENXIO); 143 } 144 dst = FWLOCALBUS | sc->fwdev->dst; 145 spd = min(sc->fwdev->speed, FWSPD_S400); 146 FWCAM_UNLOCK(sc); 147 148 err = fw_read_quadlet(sc->fd.fc, M_FWCAM, dst, spd, 149 sc->cmd_hi, sc->cmd_lo + offset, val); 150 if (err) 151 FWCAM_DEBUG(1, "read_quadlet: offset=0x%x err=%d\n", 152 offset, err); 153 return (err); 154 } 155 156 static int 157 fwcam_write_quadlet(struct fwcam_softc *sc, uint32_t offset, uint32_t val) 158 { 159 uint16_t dst; 160 uint8_t spd; 161 int err; 162 163 FWCAM_LOCK(sc); 164 if (sc->fwdev == NULL) { 165 FWCAM_UNLOCK(sc); 166 return (ENXIO); 167 } 168 dst = FWLOCALBUS | sc->fwdev->dst; 169 spd = min(sc->fwdev->speed, FWSPD_S400); 170 FWCAM_UNLOCK(sc); 171 172 err = fw_write_quadlet(sc->fd.fc, M_FWCAM, dst, spd, 173 sc->cmd_hi, sc->cmd_lo + offset, val); 174 if (err) 175 FWCAM_DEBUG(1, "write_quadlet: offset=0x%x err=%d\n", 176 offset, err); 177 return (err); 178 } 179 180 static int 181 fwcam_read_capabilities(struct fwcam_softc *sc) 182 { 183 int err, f, m; 184 185 err = fwcam_read_quadlet(sc, IIDC_V_FORMAT_INQ, &sc->formats); 186 if (err) { 187 device_printf(sc->fd.dev, 188 "failed to read V_FORMAT_INQ: %d\n", err); 189 return (err); 190 } 191 192 err = fwcam_read_quadlet(sc, IIDC_BASIC_FUNC_INQ, &sc->basic_func); 193 if (err) { 194 device_printf(sc->fd.dev, 195 "failed to read BASIC_FUNC_INQ: %d\n", err); 196 return (err); 197 } 198 199 /* Read mode and rate inquiry registers for each supported format */ 200 for (f = 0; f < 8; f++) { 201 if (!(sc->formats & (1 << (31 - f)))) { 202 sc->modes[f] = 0; 203 continue; 204 } 205 err = fwcam_read_quadlet(sc, IIDC_V_MODE_INQ(f), 206 &sc->modes[f]); 207 if (err) { 208 sc->modes[f] = 0; 209 continue; 210 } 211 for (m = 0; m < 8; m++) { 212 if (!(sc->modes[f] & (1 << (31 - m)))) { 213 sc->rates[f][m] = 0; 214 continue; 215 } 216 err = fwcam_read_quadlet(sc, IIDC_V_RATE_INQ(f, m), 217 &sc->rates[f][m]); 218 if (err) 219 sc->rates[f][m] = 0; 220 } 221 } 222 223 err = fwcam_read_quadlet(sc, IIDC_FEATURE_HI_INQ, &sc->features_hi); 224 if (err) 225 sc->features_hi = 0; 226 227 err = fwcam_read_quadlet(sc, IIDC_FEATURE_LO_INQ, &sc->features_lo); 228 if (err) 229 sc->features_lo = 0; 230 231 return (0); 232 } 233 234 static int 235 fwcam_power_on(struct fwcam_softc *sc) 236 { 237 uint32_t val; 238 int err, retries; 239 240 err = fwcam_read_quadlet(sc, IIDC_BASIC_FUNC_INQ, &val); 241 if (err) { 242 device_printf(sc->fd.dev, 243 "cannot read BASIC_FUNC_INQ: %d\n", err); 244 return (err); 245 } 246 247 if ((val & IIDC_CAM_POWER_CTRL) == 0) { 248 FWCAM_DEBUG(1, "no power control, assuming powered\n"); 249 return (0); 250 } 251 252 err = fwcam_read_quadlet(sc, IIDC_CAMERA_POWER, &val); 253 if (err == 0 && (val & IIDC_POWER_ON)) { 254 FWCAM_DEBUG(1, "camera already powered on\n"); 255 return (0); 256 } 257 258 err = fwcam_write_quadlet(sc, IIDC_CAMERA_POWER, IIDC_POWER_ON); 259 if (err) { 260 device_printf(sc->fd.dev, 261 "failed to write CAMERA_POWER: %d\n", err); 262 return (err); 263 } 264 265 for (retries = 0; retries < 50; retries++) { 266 pause("fwcampw", hz / 10); 267 268 if (sc->state == FWCAM_STATE_DETACHING) 269 return (ENXIO); 270 271 err = fwcam_read_quadlet(sc, IIDC_CAMERA_POWER, &val); 272 if (err) 273 continue; /* read may fail while powering up */ 274 275 if (val & IIDC_POWER_ON) 276 return (0); 277 } 278 279 device_printf(sc->fd.dev, "camera power-on timeout (5s)\n"); 280 return (ETIMEDOUT); 281 } 282 283 static void 284 fwcam_probe_task(void *arg, int pending __unused) 285 { 286 struct fwcam_softc *sc = (struct fwcam_softc *)arg; 287 int err; 288 289 if (sc->state == FWCAM_STATE_DETACHING || sc->fwdev == NULL) 290 return; 291 292 err = fwcam_power_on(sc); 293 if (err) { 294 device_printf(sc->fd.dev, 295 "power-on failed (%d), trying to read anyway\n", err); 296 } 297 298 if (sc->state == FWCAM_STATE_DETACHING) 299 return; 300 301 if (fwcam_read_capabilities(sc) == 0) { 302 uint32_t val; 303 304 if (fwcam_read_quadlet(sc, IIDC_CUR_V_FORMAT, &val) == 0) 305 sc->cur_format = (val >> 29) & 0x7; 306 if (fwcam_read_quadlet(sc, IIDC_CUR_V_MODE, &val) == 0) 307 sc->cur_mode = (val >> 29) & 0x7; 308 if (fwcam_read_quadlet(sc, IIDC_CUR_V_FRM_RATE, &val) == 0) 309 sc->cur_framerate = (val >> 29) & 0x7; 310 311 FWCAM_LOCK(sc); 312 if (sc->state == FWCAM_STATE_DETACHING) { 313 FWCAM_UNLOCK(sc); 314 return; 315 } 316 sc->state = FWCAM_STATE_PROBED; 317 wakeup(sc); 318 FWCAM_UNLOCK(sc); 319 320 if (sc->dma_ch >= 0 && 321 sc->state != FWCAM_STATE_DETACHING) 322 fwcam_iso_start(sc); 323 } else { 324 FWCAM_LOCK(sc); 325 if (sc->state == FWCAM_STATE_PROBING) 326 sc->state = FWCAM_STATE_IDLE; 327 wakeup(sc); 328 FWCAM_UNLOCK(sc); 329 } 330 } 331 332 /* 333 * Compute expected frame size for current format/mode. 334 * Format_0 (VGA) modes: 335 * Mode 0: 160x120 YUV444 = 160*120*3 = 57600 336 * Mode 1: 320x240 YUV422 = 320*240*2 = 153600 337 * Mode 2: 640x480 YUV411 = 640*480*3/2 = 460800 338 * Mode 3: 640x480 YUV422 = 640*480*2 = 614400 339 * Mode 4: 640x480 RGB8 = 640*480*3 = 921600 340 * Mode 5: 640x480 Mono8 = 640*480 = 307200 341 */ 342 static uint32_t 343 fwcam_frame_size(struct fwcam_softc *sc) 344 { 345 static const uint32_t fmt0_sizes[] = { 346 160 * 120 * 3, /* mode 0: YUV444 */ 347 320 * 240 * 2, /* mode 1: YUV422 */ 348 640 * 480 * 3 / 2, /* mode 2: YUV411 */ 349 640 * 480 * 2, /* mode 3: YUV422 */ 350 640 * 480 * 3, /* mode 4: RGB8 */ 351 640 * 480, /* mode 5: Mono8 */ 352 }; 353 354 if (sc->cur_format == IIDC_FMT_VGA && sc->cur_mode < nitems(fmt0_sizes)) 355 return (fmt0_sizes[sc->cur_mode]); 356 357 /* Default to largest VGA mode */ 358 return (FWCAM_MAX_FRAME_SIZE); 359 } 360 361 static int 362 fwcam_iso_start(struct fwcam_softc *sc) 363 { 364 struct firewire_comm *fc = sc->fd.fc; 365 struct fw_xferq *xferq; 366 uint32_t val; 367 int dma_ch, err; 368 369 mtx_assert(&sc->mtx, MA_NOTOWNED); 370 371 FWCAM_LOCK(sc); 372 if (sc->dma_ch >= 0 || sc->state == FWCAM_STATE_STREAMING) { 373 FWCAM_UNLOCK(sc); 374 return (0); /* already running or starting */ 375 } 376 if (sc->state == FWCAM_STATE_DETACHING) { 377 FWCAM_UNLOCK(sc); 378 return (ENXIO); 379 } 380 FWCAM_UNLOCK(sc); 381 382 dma_ch = fw_open_isodma(fc, 0); 383 if (dma_ch < 0) { 384 device_printf(sc->fd.dev, "no IR DMA channel available\n"); 385 return (EBUSY); 386 } 387 388 FWCAM_LOCK(sc); 389 if (sc->dma_ch >= 0) { 390 FWCAM_UNLOCK(sc); 391 fc->ir[dma_ch]->flag &= ~FWXFERQ_OPEN; 392 return (0); 393 } 394 FWCAM_UNLOCK(sc); 395 396 xferq = fc->ir[dma_ch]; 397 xferq->flag |= FWXFERQ_EXTBUF | FWXFERQ_HANDLER | FWXFERQ_STREAM; 398 399 sc->iso_channel = (uint8_t)(iso_channel & FWXFERQ_CHTAGMASK); 400 xferq->flag &= ~FWXFERQ_CHTAGMASK; 401 xferq->flag |= sc->iso_channel & FWXFERQ_CHTAGMASK; 402 403 xferq->sc = (caddr_t)sc; 404 xferq->hand = fwcam_iso_input; 405 xferq->bnchunk = FWCAM_ISO_NCHUNK; 406 xferq->bnpacket = 1; 407 xferq->psize = FWCAM_ISO_PKTSIZE; 408 xferq->queued = 0; 409 xferq->buf = NULL; 410 411 xferq->bulkxfer = malloc(sizeof(struct fw_bulkxfer) * xferq->bnchunk, 412 M_FWCAM, M_WAITOK | M_ZERO); 413 414 fw_iso_init_chunks(xferq); 415 416 sc->frame_size = fwcam_frame_size(sc); 417 sc->frame_buf = malloc(sc->frame_size, M_FWCAM, M_WAITOK | M_ZERO); 418 sc->read_buf = malloc(sc->frame_size, M_FWCAM, M_WAITOK | M_ZERO); 419 sc->frame_offset = 0; 420 sc->frame_ready = 0; 421 sc->frame_dropped = 0; 422 423 /* IIDC spec s3.1: set video mode registers before ISO enable */ 424 err = fwcam_write_quadlet(sc, IIDC_CUR_V_FORMAT, 425 (uint32_t)sc->cur_format << IIDC_CUR_V_SHIFT); 426 if (err) { 427 device_printf(sc->fd.dev, 428 "failed to set CUR_V_FORMAT: %d\n", err); 429 goto fail; 430 } 431 err = fwcam_write_quadlet(sc, IIDC_CUR_V_MODE, 432 (uint32_t)sc->cur_mode << IIDC_CUR_V_SHIFT); 433 if (err) { 434 device_printf(sc->fd.dev, 435 "failed to set CUR_V_MODE: %d\n", err); 436 goto fail; 437 } 438 err = fwcam_write_quadlet(sc, IIDC_CUR_V_FRM_RATE, 439 (uint32_t)sc->cur_framerate << IIDC_CUR_V_SHIFT); 440 if (err) { 441 device_printf(sc->fd.dev, 442 "failed to set CUR_V_FRM_RATE: %d\n", err); 443 goto fail; 444 } 445 446 /* Check Vmode_Error_Status - camera rejects ISO_EN on error */ 447 err = fwcam_read_quadlet(sc, IIDC_VMODE_ERR_STATUS, &val); 448 if (err == 0 && (val & IIDC_VMODE_ERROR)) { 449 device_printf(sc->fd.dev, 450 "Vmode_Error_Status set: format=%d mode=%d rate=%d " 451 "speed=%d\n", sc->cur_format, sc->cur_mode, 452 sc->cur_framerate, sc->iso_speed); 453 err = EINVAL; 454 goto fail; 455 } 456 457 val = ((uint32_t)sc->iso_channel << IIDC_ISO_CH_SHIFT) | 458 ((uint32_t)sc->iso_speed << IIDC_ISO_SPEED_SHIFT); 459 err = fwcam_write_quadlet(sc, IIDC_ISO_CHANNEL, val); 460 if (err) { 461 device_printf(sc->fd.dev, 462 "failed to set ISO_CHANNEL: %d\n", err); 463 goto fail; 464 } 465 466 err = fwcam_write_quadlet(sc, IIDC_ISO_EN, IIDC_ISO_EN_ON); 467 if (err == EIO) { 468 /* 469 * Cameras with a lens cover might power down the sensor 470 * when the cover is closed and reject streaming requests. 471 * Try to re-power and retry once before giving up. 472 */ 473 err = fwcam_power_on(sc); 474 if (err == 0) 475 err = fwcam_write_quadlet(sc, IIDC_ISO_EN, 476 IIDC_ISO_EN_ON); 477 if (err) { 478 device_printf(sc->fd.dev, 479 "ISO enable refused (lens cover closed?)\n"); 480 goto fail; 481 } 482 } else if (err) { 483 device_printf(sc->fd.dev, 484 "failed to enable ISO: %d\n", err); 485 goto fail; 486 } 487 488 err = fc->irx_enable(fc, dma_ch); 489 if (err) { 490 device_printf(sc->fd.dev, 491 "failed to start IR DMA: %d\n", err); 492 fwcam_write_quadlet(sc, IIDC_ISO_EN, 0); 493 goto fail; 494 } 495 496 FWCAM_LOCK(sc); 497 if (sc->state == FWCAM_STATE_DETACHING) { 498 FWCAM_UNLOCK(sc); 499 fc->irx_disable(fc, dma_ch); 500 fwcam_write_quadlet(sc, IIDC_ISO_EN, 0); 501 err = ENXIO; 502 goto fail; 503 } 504 sc->dma_ch = dma_ch; 505 sc->state = FWCAM_STATE_STREAMING; 506 FWCAM_UNLOCK(sc); 507 508 return (0); 509 510 fail: 511 fw_iso_free_chunks(xferq, M_FWCAM); 512 xferq->flag &= ~(FWXFERQ_MODEMASK | FWXFERQ_OPEN | FWXFERQ_STREAM | 513 FWXFERQ_EXTBUF | FWXFERQ_HANDLER | FWXFERQ_CHTAGMASK); 514 xferq->hand = NULL; 515 516 free(sc->frame_buf, M_FWCAM); 517 free(sc->read_buf, M_FWCAM); 518 sc->frame_buf = NULL; 519 sc->read_buf = NULL; 520 sc->dma_ch = -1; 521 522 return (err); 523 } 524 525 static void 526 fwcam_iso_stop(struct fwcam_softc *sc) 527 { 528 struct firewire_comm *fc = sc->fd.fc; 529 struct fw_xferq *xferq; 530 int dma_ch; 531 532 FWCAM_LOCK(sc); 533 dma_ch = sc->dma_ch; 534 if (dma_ch < 0) { 535 FWCAM_UNLOCK(sc); 536 return; 537 } 538 sc->dma_ch = -1; /* claim ownership of teardown */ 539 FWCAM_UNLOCK(sc); 540 541 xferq = fc->ir[dma_ch]; 542 543 if (xferq->flag & FWXFERQ_RUNNING) 544 fc->irx_disable(fc, dma_ch); 545 546 if (sc->fwdev != NULL) 547 fwcam_write_quadlet(sc, IIDC_ISO_EN, 0); 548 549 FWCAM_LOCK(sc); 550 fw_iso_wait_inactive_locked(&sc->mtx, &sc->iso_active, "fwcamis"); 551 sc->frame_ready = 0; 552 while (sc->read_in_progress) 553 msleep(&sc->read_in_progress, &sc->mtx, PWAIT, "fwcamst", hz); 554 FWCAM_UNLOCK(sc); 555 556 xferq->flag &= ~(FWXFERQ_MODEMASK | FWXFERQ_OPEN | FWXFERQ_STREAM | 557 FWXFERQ_EXTBUF | FWXFERQ_HANDLER | FWXFERQ_CHTAGMASK); 558 xferq->hand = NULL; 559 560 fw_iso_free_chunks(xferq, M_FWCAM); 561 562 free(sc->frame_buf, M_FWCAM); 563 free(sc->read_buf, M_FWCAM); 564 sc->frame_buf = NULL; 565 sc->read_buf = NULL; 566 } 567 568 static void 569 fwcam_iso_input(struct fw_xferq *xferq) 570 { 571 struct fwcam_softc *sc = (struct fwcam_softc *)xferq->sc; 572 struct fw_bulkxfer *sxfer; 573 struct fw_pkt *fp; 574 struct mbuf *m; 575 uint8_t *payload; 576 uint32_t plen; 577 uint8_t *tmp; 578 int dma_ch; 579 580 FWCAM_LOCK(sc); 581 dma_ch = sc->dma_ch; 582 if (dma_ch < 0 || sc->frame_buf == NULL) { 583 FWCAM_UNLOCK(sc); 584 return; 585 } 586 sc->iso_active = 1; 587 FWCAM_UNLOCK(sc); 588 589 while ((sxfer = STAILQ_FIRST(&xferq->stvalid)) != NULL) { 590 STAILQ_REMOVE_HEAD(&xferq->stvalid, link); 591 592 m = fw_iso_dequeue(xferq, sxfer, sc->fd.fc); 593 if (m == NULL) 594 continue; 595 596 fp = mtod(m, struct fw_pkt *); 597 plen = fp->mode.stream.len; 598 if (plen == 0) { 599 m_freem(m); 600 continue; /* empty packet (padding) */ 601 } 602 603 if (fp->mode.stream.sy == 1) { 604 if (sc->frame_offset > 0) { 605 if (sc->frame_offset == sc->frame_size) { 606 FWCAM_LOCK(sc); 607 if (sc->read_in_progress) { 608 sc->frame_dropped++; 609 } else { 610 if (sc->frame_ready) 611 sc->frame_dropped++; 612 tmp = sc->read_buf; 613 sc->read_buf = sc->frame_buf; 614 sc->frame_buf = tmp; 615 sc->frame_ready = 1; 616 wakeup(sc); 617 selwakeup(&sc->rsel); 618 } 619 FWCAM_UNLOCK(sc); 620 } else { 621 sc->frame_dropped++; 622 } 623 } 624 sc->frame_offset = 0; 625 } 626 627 if (m->m_len < 4) { 628 m_freem(m); 629 continue; 630 } 631 payload = mtod(m, uint8_t *) + 4; 632 if (plen > (uint32_t)(m->m_len - 4)) 633 plen = m->m_len - 4; 634 635 if (sc->frame_offset + plen <= sc->frame_size) { 636 memcpy(sc->frame_buf + sc->frame_offset, payload, plen); 637 sc->frame_offset += plen; 638 } else { 639 sc->frame_dropped++; 640 sc->frame_offset = 0; 641 } 642 643 m_freem(m); 644 } 645 646 fw_iso_rearm_done(xferq, sc->fd.fc, &sc->mtx, &sc->iso_active, 647 &sc->dma_ch, dma_ch); 648 } 649 650 static int 651 fwcam_cdev_open(struct cdev *dev, int oflags, int devtype, struct thread *td) 652 { 653 struct fwcam_softc *sc = dev->si_drv1; 654 int err; 655 656 FWCAM_LOCK(sc); 657 if (sc->state == FWCAM_STATE_DETACHING) { 658 FWCAM_UNLOCK(sc); 659 return (ENXIO); 660 } 661 662 if (sc->state == FWCAM_STATE_IDLE) { 663 sc->state = FWCAM_STATE_PROBING; 664 FWCAM_UNLOCK(sc); 665 taskqueue_enqueue(taskqueue_thread, &sc->probe_task); 666 FWCAM_LOCK(sc); 667 } 668 669 while (sc->state == FWCAM_STATE_PROBING) { 670 err = msleep(sc, &sc->mtx, PCATCH, "fwcampr", 10 * hz); 671 if (err) { 672 FWCAM_UNLOCK(sc); 673 return (err == EWOULDBLOCK ? ETIMEDOUT : err); 674 } 675 } 676 677 if (sc->state != FWCAM_STATE_PROBED && 678 sc->state != FWCAM_STATE_STREAMING) { 679 FWCAM_UNLOCK(sc); 680 return (ENXIO); 681 } 682 683 sc->open_count++; 684 FWCAM_UNLOCK(sc); 685 return (0); 686 } 687 688 static int 689 fwcam_cdev_close(struct cdev *dev, int fflag, int devtype, struct thread *td) 690 { 691 struct fwcam_softc *sc = dev->si_drv1; 692 693 FWCAM_LOCK(sc); 694 sc->open_count--; 695 if (sc->open_count <= 0) { 696 sc->open_count = 0; 697 if (sc->state == FWCAM_STATE_STREAMING) { 698 FWCAM_UNLOCK(sc); 699 fwcam_iso_stop(sc); 700 FWCAM_LOCK(sc); 701 if (sc->state != FWCAM_STATE_DETACHING) 702 sc->state = FWCAM_STATE_PROBED; 703 } 704 } 705 FWCAM_UNLOCK(sc); 706 return (0); 707 } 708 709 static int 710 fwcam_cdev_read(struct cdev *dev, struct uio *uio, int ioflag) 711 { 712 struct fwcam_softc *sc = dev->si_drv1; 713 int err; 714 715 FWCAM_LOCK(sc); 716 if (sc->state == FWCAM_STATE_PROBED) { 717 FWCAM_UNLOCK(sc); 718 err = fwcam_iso_start(sc); 719 if (err) 720 return (err); 721 FWCAM_LOCK(sc); 722 } 723 while (!sc->frame_ready) { 724 if (sc->state != FWCAM_STATE_STREAMING) { 725 FWCAM_UNLOCK(sc); 726 return (ENXIO); 727 } 728 if (ioflag & FNONBLOCK) { 729 FWCAM_UNLOCK(sc); 730 return (EAGAIN); 731 } 732 err = msleep(sc, &sc->mtx, PCATCH, "fwcamrd", 5 * hz); 733 if (err) { 734 FWCAM_UNLOCK(sc); 735 return (err); 736 } 737 } 738 739 sc->frame_ready = 0; 740 if (sc->read_buf == NULL) { 741 FWCAM_UNLOCK(sc); 742 return (ENXIO); 743 } 744 sc->read_in_progress = 1; 745 FWCAM_UNLOCK(sc); 746 747 err = uiomove(sc->read_buf, 748 MIN(uio->uio_resid, sc->frame_size), uio); 749 750 FWCAM_LOCK(sc); 751 sc->read_in_progress = 0; 752 wakeup(&sc->read_in_progress); 753 FWCAM_UNLOCK(sc); 754 755 return (err); 756 } 757 758 static int 759 fwcam_cdev_poll(struct cdev *dev, int events, struct thread *td) 760 { 761 struct fwcam_softc *sc = dev->si_drv1; 762 int revents = 0; 763 764 FWCAM_LOCK(sc); 765 if (events & (POLLIN | POLLRDNORM)) { 766 if (sc->frame_ready) 767 revents |= events & (POLLIN | POLLRDNORM); 768 else 769 selrecord(td, &sc->rsel); 770 } 771 FWCAM_UNLOCK(sc); 772 return (revents); 773 } 774 775 static const uint32_t fwcam_feat_inq[] = { 776 [FWCAM_FEAT_BRIGHTNESS] = IIDC_BRIGHTNESS_INQ, 777 [FWCAM_FEAT_AUTO_EXPOSURE] = IIDC_AUTO_EXPOSURE_INQ, 778 [FWCAM_FEAT_SHARPNESS] = IIDC_SHARPNESS_INQ, 779 [FWCAM_FEAT_WHITE_BALANCE] = IIDC_WHITE_BAL_INQ, 780 [FWCAM_FEAT_HUE] = IIDC_HUE_INQ, 781 [FWCAM_FEAT_SATURATION] = IIDC_SATURATION_INQ, 782 [FWCAM_FEAT_GAMMA] = IIDC_GAMMA_INQ, 783 [FWCAM_FEAT_SHUTTER] = IIDC_SHUTTER_INQ, 784 [FWCAM_FEAT_GAIN] = IIDC_GAIN_INQ, 785 [FWCAM_FEAT_IRIS] = IIDC_IRIS_INQ, 786 [FWCAM_FEAT_FOCUS] = IIDC_FOCUS_INQ, 787 [FWCAM_FEAT_TEMPERATURE] = IIDC_TEMPERATURE_INQ, 788 [FWCAM_FEAT_TRIGGER] = IIDC_TRIGGER_INQ, 789 }; 790 791 static const uint32_t fwcam_feat_ctrl[] = { 792 [FWCAM_FEAT_BRIGHTNESS] = IIDC_BRIGHTNESS, 793 [FWCAM_FEAT_AUTO_EXPOSURE] = IIDC_AUTO_EXPOSURE, 794 [FWCAM_FEAT_SHARPNESS] = IIDC_SHARPNESS, 795 [FWCAM_FEAT_WHITE_BALANCE] = IIDC_WHITE_BALANCE, 796 [FWCAM_FEAT_HUE] = IIDC_HUE, 797 [FWCAM_FEAT_SATURATION] = IIDC_SATURATION, 798 [FWCAM_FEAT_GAMMA] = IIDC_GAMMA, 799 [FWCAM_FEAT_SHUTTER] = IIDC_SHUTTER, 800 [FWCAM_FEAT_GAIN] = IIDC_GAIN, 801 [FWCAM_FEAT_IRIS] = IIDC_IRIS, 802 [FWCAM_FEAT_FOCUS] = IIDC_FOCUS, 803 [FWCAM_FEAT_TEMPERATURE] = IIDC_TEMPERATURE, 804 [FWCAM_FEAT_TRIGGER] = IIDC_TRIGGER_MODE, 805 [FWCAM_FEAT_ZOOM] = IIDC_ZOOM, 806 [FWCAM_FEAT_PAN] = IIDC_PAN, 807 [FWCAM_FEAT_TILT] = IIDC_TILT, 808 }; 809 810 static int 811 fwcam_get_feature(struct fwcam_softc *sc, struct fwcam_feature *feat) 812 { 813 uint32_t inq, ctrl; 814 int err; 815 816 if (feat->id >= FWCAM_FEAT_MAX) 817 return (EINVAL); 818 819 feat->flags = 0; 820 feat->min = 0; 821 feat->max = 0; 822 if (feat->id < nitems(fwcam_feat_inq) && 823 fwcam_feat_inq[feat->id] != 0) { 824 err = fwcam_read_quadlet(sc, fwcam_feat_inq[feat->id], &inq); 825 if (err) 826 return (err); 827 828 if (inq & (1 << 31)) 829 feat->flags |= FWCAM_FEATF_PRESENT; 830 if (inq & (1 << 28)) 831 feat->flags |= FWCAM_FEATF_ONOFF; 832 if (inq & (1 << 27)) 833 feat->flags |= FWCAM_FEATF_AUTO; 834 if (inq & (1 << 26)) 835 feat->flags |= FWCAM_FEATF_MANUAL; 836 feat->min = (inq >> 12) & 0xfff; 837 feat->max = inq & 0xfff; 838 } 839 840 if (feat->id >= nitems(fwcam_feat_ctrl) || 841 fwcam_feat_ctrl[feat->id] == 0) 842 return (EINVAL); 843 844 err = fwcam_read_quadlet(sc, fwcam_feat_ctrl[feat->id], &ctrl); 845 if (err) 846 return (err); 847 848 feat->value = ctrl & 0xfff; 849 /* White balance has U/B in bits [20:31] and V/R in bits [8:19] */ 850 if (feat->id == FWCAM_FEAT_WHITE_BALANCE) 851 feat->value2 = (ctrl >> 12) & 0xfff; 852 else 853 feat->value2 = 0; 854 855 return (0); 856 } 857 858 static int 859 fwcam_set_feature(struct fwcam_softc *sc, struct fwcam_feature *feat) 860 { 861 uint32_t ctrl, val; 862 int err; 863 864 if (feat->id >= FWCAM_FEAT_MAX) 865 return (EINVAL); 866 if (feat->id >= nitems(fwcam_feat_ctrl) || 867 fwcam_feat_ctrl[feat->id] == 0) 868 return (EINVAL); 869 870 err = fwcam_read_quadlet(sc, fwcam_feat_ctrl[feat->id], &ctrl); 871 if (err) 872 return (err); 873 874 if (feat->id == FWCAM_FEAT_WHITE_BALANCE) { 875 /* White balance: value=V/R (low 12), value2=U/B (high 12) */ 876 val = (ctrl & 0xff000000) | 877 ((feat->value2 & 0xfff) << 12) | 878 (feat->value & 0xfff); 879 } else { 880 /* Preserve upper bits, set new value in lower 12 */ 881 val = (ctrl & 0xfffff000) | (feat->value & 0xfff); 882 } 883 884 return (fwcam_write_quadlet(sc, fwcam_feat_ctrl[feat->id], val)); 885 } 886 887 static int 888 fwcam_cdev_ioctl(struct cdev *dev, u_long cmd, caddr_t data, 889 int fflag, struct thread *td) 890 { 891 struct fwcam_softc *sc = dev->si_drv1; 892 struct fwcam_mode *mode; 893 struct fwcam_feature *feat; 894 struct fwcam_info *info; 895 int err; 896 897 if (sc->fwdev == NULL) 898 return (ENXIO); 899 900 switch (cmd) { 901 case FWCAM_GMODE: 902 mode = (struct fwcam_mode *)data; 903 mode->format = sc->cur_format; 904 mode->mode = sc->cur_mode; 905 mode->framerate = sc->cur_framerate; 906 mode->frame_size = sc->frame_size ? 907 sc->frame_size : fwcam_frame_size(sc); 908 return (0); 909 910 case FWCAM_SMODE: 911 { 912 int was_streaming = 0; 913 914 mode = (struct fwcam_mode *)data; 915 if (mode->format > 7 || mode->mode > 7 || mode->framerate > 7) 916 return (EINVAL); 917 918 if (!(sc->formats & (1 << (31 - mode->format)))) 919 return (EINVAL); 920 if (!(sc->modes[mode->format] & (1 << (31 - mode->mode)))) 921 return (EINVAL); 922 if (!(sc->rates[mode->format][mode->mode] & 923 (1 << (31 - mode->framerate)))) 924 return (EINVAL); 925 926 FWCAM_LOCK(sc); 927 if (sc->state == FWCAM_STATE_DETACHING) { 928 FWCAM_UNLOCK(sc); 929 return (ENXIO); 930 } 931 if (sc->state == FWCAM_STATE_STREAMING) { 932 was_streaming = 1; 933 FWCAM_UNLOCK(sc); 934 fwcam_iso_stop(sc); 935 FWCAM_LOCK(sc); 936 if (sc->state != FWCAM_STATE_DETACHING) 937 sc->state = FWCAM_STATE_PROBED; 938 } 939 FWCAM_UNLOCK(sc); 940 941 err = fwcam_write_quadlet(sc, IIDC_CUR_V_FORMAT, 942 (uint32_t)mode->format << IIDC_CUR_V_SHIFT); 943 if (err == 0) 944 err = fwcam_write_quadlet(sc, IIDC_CUR_V_MODE, 945 (uint32_t)mode->mode << IIDC_CUR_V_SHIFT); 946 if (err == 0) 947 err = fwcam_write_quadlet(sc, IIDC_CUR_V_FRM_RATE, 948 (uint32_t)mode->framerate << IIDC_CUR_V_SHIFT); 949 950 if (err == 0) { 951 sc->cur_format = mode->format; 952 sc->cur_mode = mode->mode; 953 sc->cur_framerate = mode->framerate; 954 mode->frame_size = fwcam_frame_size(sc); 955 } 956 957 if (was_streaming) 958 fwcam_iso_start(sc); 959 return (err); 960 } 961 962 case FWCAM_GFEAT: 963 feat = (struct fwcam_feature *)data; 964 return (fwcam_get_feature(sc, feat)); 965 966 case FWCAM_SFEAT: 967 feat = (struct fwcam_feature *)data; 968 return (fwcam_set_feature(sc, feat)); 969 970 case FWCAM_GINFO: 971 info = (struct fwcam_info *)data; 972 info->formats = sc->formats; 973 info->basic_func = sc->basic_func; 974 info->features_hi = sc->features_hi; 975 info->features_lo = sc->features_lo; 976 info->cur_format = sc->cur_format; 977 info->cur_mode = sc->cur_mode; 978 info->cur_framerate = sc->cur_framerate; 979 info->state = sc->state; 980 info->frame_size = sc->frame_size ? 981 sc->frame_size : fwcam_frame_size(sc); 982 info->frame_dropped = sc->frame_dropped; 983 info->iso_channel = sc->iso_channel; 984 info->_pad[0] = info->_pad[1] = info->_pad[2] = 0; 985 return (0); 986 987 default: 988 return (ENOTTY); 989 } 990 } 991 992 993 static int 994 fwcam_probe(device_t dev) 995 { 996 struct fw_unit *unit; 997 998 unit = fw_get_unit(dev); 999 if (unit == NULL) 1000 return (ENXIO); 1001 1002 if (unit->spec_id != CSRVAL_1394TA) 1003 return (ENXIO); 1004 if (unit->sw_version != CSR_PROTCAM130 && 1005 unit->sw_version != CSR_PROTCAM120 && 1006 unit->sw_version != CSR_PROTCAM104) 1007 return (ENXIO); 1008 1009 device_set_desc(dev, "IIDC Digital Camera over FireWire"); 1010 return (BUS_PROBE_DEFAULT); 1011 } 1012 1013 static int 1014 fwcam_attach(device_t dev) 1015 { 1016 struct fwcam_softc *sc; 1017 struct fw_unit *unit; 1018 uint32_t cmd_base; 1019 1020 unit = fw_get_unit(dev); 1021 if (unit == NULL || unit->fwdev == NULL) 1022 return (ENXIO); 1023 1024 cmd_base = fwcam_find_iidc_cmd_base(unit); 1025 if (cmd_base == 0) { 1026 device_printf(dev, "no IIDC command base in unit directory\n"); 1027 return (ENXIO); 1028 } 1029 1030 sc = device_get_softc(dev); 1031 sc->fd.dev = dev; 1032 sc->fd.fc = fw_get_comm(dev); 1033 mtx_init(&sc->mtx, "fwcam", NULL, MTX_DEF); 1034 1035 sc->fwdev = unit->fwdev; 1036 sc->cmd_hi = 0xffff; 1037 sc->cmd_lo = 0xf0000000 | (cmd_base << 2); 1038 sc->iso_speed = min(unit->fwdev->speed, FWSPD_S400); 1039 sc->state = FWCAM_STATE_IDLE; 1040 sc->dma_ch = -1; 1041 sc->iso_active = 0; 1042 sc->open_count = 0; 1043 knlist_init_mtx(&sc->rsel.si_note, &sc->mtx); 1044 TASK_INIT(&sc->probe_task, 0, fwcam_probe_task, sc); 1045 1046 sc->cdev = make_dev(&fwcam_cdevsw, device_get_unit(dev), 1047 UID_ROOT, GID_OPERATOR, 0660, "fwcam%d", device_get_unit(dev)); 1048 sc->cdev->si_drv1 = sc; 1049 1050 return (0); 1051 } 1052 1053 static int 1054 fwcam_detach(device_t dev) 1055 { 1056 struct fwcam_softc *sc; 1057 1058 sc = device_get_softc(dev); 1059 1060 FWCAM_LOCK(sc); 1061 sc->state = FWCAM_STATE_DETACHING; 1062 wakeup(sc); /* wake any sleeping readers */ 1063 FWCAM_UNLOCK(sc); 1064 1065 taskqueue_drain(taskqueue_thread, &sc->probe_task); 1066 fwcam_iso_stop(sc); 1067 1068 if (sc->cdev != NULL) 1069 destroy_dev(sc->cdev); 1070 1071 seldrain(&sc->rsel); 1072 knlist_destroy(&sc->rsel.si_note); 1073 1074 FWCAM_LOCK(sc); 1075 sc->fwdev = NULL; 1076 FWCAM_UNLOCK(sc); 1077 1078 mtx_destroy(&sc->mtx); 1079 1080 return (0); 1081 } 1082 1083 static device_method_t fwcam_methods[] = { 1084 DEVMETHOD(device_probe, fwcam_probe), 1085 DEVMETHOD(device_attach, fwcam_attach), 1086 DEVMETHOD(device_detach, fwcam_detach), 1087 1088 DEVMETHOD_END 1089 }; 1090 1091 static driver_t fwcam_driver = { 1092 "fwcam", 1093 fwcam_methods, 1094 sizeof(struct fwcam_softc), 1095 }; 1096 1097 DRIVER_MODULE(fwcam, firewire, fwcam_driver, 0, 0); 1098 MODULE_VERSION(fwcam, 1); 1099 MODULE_DEPEND(fwcam, firewire, 1, 1, 1); 1100